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ABSTRACT 

A  sliding  mode  compensator  for  depth  control  of  an  autonomous  underwater 
vehicle  (AUV)  using  depth  feedback  only  is  designed.  The  controller  is  evaluated 
for  a  nominal  linear  model  and  optimized  by  a  series  of  numerical  experiments 
for  a  number  of  depth  changing  maneuvers.  A  state  observer  is  used  in  order  to 
estimate  the  unmeasurable  states  together  with  the  sliding  mode  controller.  The 
effects  of  varying  control  parameters  are  discussed.  Compensator  performance 
is  assessed  by  numerical  simulation  of  AUV  dynamic  response  based  on  the  full 
six  degrees  of  freedom  nonlinear  equations  of  motion.  The  expected  robustness 
of  the  design  is  demonstrated  by  comparison  between  linear  and  nonlinear  vehicle 
response  characteristics,  and  by  a  wide  variation  in  vehicle  parameters  and 
h\'drodynamic  coefficients.  Finally,  suggestions  for  design  improvement  and  di- 
rections for  future  research  are  indicated. 
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I.     INTRODUCTION 

A.  GENERAL 

There  has  been  an  increased  interest  recently  in  the  need  for  autonomous 
underwater  vehicles  (AUV)  in  both  Naw  and  private  industry.  A  variety  of  un- 
classified missions  includes  ASW,  decoy,  survey,  reconnaissance,  and  ocean  engi- 
neering work  service.  As  the  cost  of  manned  submarine  vehicles  increases,  there 
are  significant  advantages  to  the  use  of  cheaper  unmanned  vehicles.  The  AUV 
should  be  able  to  maneuxer  freely  in  the  ocean  enviroment  with  respect  to  depth, 
heading,  and  speed  in  order  to  carry  out  its  missions.  Such  maneuvering  require- 
ments have  to  be  easily  accomplished  by  a  low  level  active  control  system,  and  in 
the  presence  of  environmental  and  physical  uncertainty. 

All  information  concerning  the  environment  of  a  vehicle  is  detected  by  the  sens- 
ing lc\el  o[  control  on-board  the  \ehicle  and  directed  to  the  high  Ie\el  intelligent 
system  in  order  to  carry  out  an  unmanned  mission.  The  dynamics  of  underwater 
\ehicles  are  described  by  highly  nonlinear  systems  with  uncertain  coefficients  and 
disturbances  that  are  difficult  to  measure.  Robust  control  using  Nariable  structure 
systems  are  reputed  to  pro\ide  accurate  control  of  nonlinear  systems  despite  un- 
modeled  system  dynamics  and  disturbances,  leading  to  the  motion  that  sliding 
mode  compensators  should  be  employed  in  situations  where  accurate  tracking  is 
desired  and  where  mancu\ering  parameters  of  AUV  change  with  operating  con- 
ditions. 

B.  AIM  OF  THIS  STUDY 

This  thesis  aims  at  in\estigation  ol^  the  use  of  sliding  mode  compensator  for 
AUV  depth  keeping  and  changing.  The  control  concept  dc\eloped  here  is  that 
of  a  N'ariablc  structure  system  consisting  of  continuous  subsystems  together  with 
suitable  switching  logic.  The  sliding  mode  control  concept  was  suggested  by  V. 
Utkin  [Rcfi  1]  and  recently  de\eloped  by  J.J.E.  Slotine  [Refi  2].  Because  sliding 
mode  control  requires  full  state  feedback,  this  work  has  incorporated  a  state  ob- 
ser\ed  based  on  output  measurement  resulting  in  a  sliding  mode  compensator. 


The  main  goal  of  this  thesis  is  to  present  a  design  procedure  and  estimate 
robustness  of  the  variable  structure  compensator  in  the  presence  of  vehicle  non- 
linearities,  modeling  errors,  uncertainties,  and  variation  of  parameters. 

C.     THESIS  OUTLINE 

Chapter  2  introduces  the  basic  concept  of  Liapunov  stability  and  an 
asymptotically  stable  condition  which  is  related  to  energy  degeneration  with  in- 
creasing time  for  a  dynamics  system.  The  other  sections  will  discuss  how  to  de- 
sign a  sliding  surface  and  a  control  law  based  on  a  linear  model.  The  last  section 
of  the  chapter  presents  a  technique  to  eliminate  chattering  in  order  to  provide 
smooth  control  inputs. 

In  Chapter  3,  vehicle  dynamics  and  a  process  used  to  produce  a  linear  state 
space  representation  are  described.  Sliding  control  law  for  a  linear  model  is  de- 
signed using  results  of  the  previous  chapter.  The  design  is  evaluated  through 
computer  simulation. 

Chapter  4  presents  the  sliding  mode  compensator  using  depth  measurements 
only.  A  model  with  perturbed  hydrodynamic  and  geometric  parameters  is  used 
to  estimate  the  performance  of  the  sliding  mode  compensator.  The  chapter  ends 
with  a  discussion  of  the  robustness  of  the  sliding  mode  compensator  and  advan- 
tage of  the  \ariable  structure  control  system 

Finally.  Chapter  5  contains  a  summary,  conclusions,  and  some  directions  for 
further  research. 


II.     THEORETICAL  BACKGROUND  OF  SLIDING  MODE  CONTROL 

A.     GENERAL 

The  dynamics  of  underwater  vehicles  are  described  by  highly  nonlinear,  high 
order  systems  with  uncertain  models  and  disturbances  that  are  difficult  to  model. 
A  new  form  of  sliding  mode  (Variable  Structure  System)  control  has  been  devel- 
oped recently,  and  shown  to  apply  to  a  large  class  of  nonlinear  systems  [Ref.  3]. 
Sliding  mode  control  offers  the  control  designer  new  possibilities  for  improving 
the  quality  of  the  control  in  comparison  with  a  fixed  structure  system.  The  basic 
idea  is  to  design  a  controller  structure  which  consists  of  a  set  of  continuous  sub- 
systems together  with  suitable  switching  logic  according  to  [Ref.  1].  The  basic 
sliding  mode  control  for  a  SISO  svstem 


X  =  Ax+Bu 

w  =  [  +  4^A-]  (2.1) 

where.        x   is   the  state  variable 

u   is   the  sliding  mode  control  law 
M-*  is  a  switched  feedback  gain 
A.B   are   system  matrix. 

This  chapter  is  devoted  to  the  study  of  the  basic  background  of  the  sliding 
mode  theory  for  the  design  of  a  linear  controller  for  the  AUV. 

B.     LIAPUNOV  STABILITY 

For  a  gi\en  control  system,  stability  is  usually  the  most  important  thing  to 
be  determined.  If  the  system  is  linear  and  time  in\'ariant.  then,  many  stability 
criteria  are  a\ailablc.  such  as  the  N'N'quist  stability  criterion,  the  Routh's  stability 


criterion  etc.  The  second  method  of  Liapunov  is  the  most  general  method  for  the 
determination  of  the  stabihty  of  nonhnear  and  time  varying  systems.  Before  dis- 
cussing the  sHding  mode  control,  the  second  method  of  Liapunov  will  be  dis- 
cussed in  order  to  understand  the  sliding  condition,  which  will  be  discussed  in  the 
next  section.  The  basic  concept  of  the  second  method  of  Liapunov  is  that  if  the 
system  has  an  asymptotically  stable  equilibrium  state,  the  stored  energy  of  system 
decays  with  increasing  time  until  it  finally  assumes  it's  minimum  value.  In  order 
to  explain  this,  Liapunov  introduced  the  so  called  Liapunov  function,  an  imagi- 
nary energy  function  which  depends  on  the  state  variable  (x,.X2...jcJ  and  time  (t). 
If  the  LiapunoN'  function  is  denoted  by  V(x,t)  and  it's  time  derivatives  denoted 
by 

dvixj) 
V{x,t)  =  — ^  (2.2) 

at 

then  the  Liapunov  function  has  information  as  to  stability,  asymptotic  stability 
or  instability  of  an  equilibrium  state  of  the  system  without  solving  the  state 
equation.  The  theorem  of  the  Liapuno\'  function  is  described  in  modern  control 
engineering   [Ref.  4].    If  a  system  is  described  by 


A-=/(A-,0  (2.3) 

where    x  is  the  state  \  ariable  and  if 
f(0,t)  =  0,  for  rp  <  t. 

and  there  exists  a  Liapuno\-  function  V(x.t)  ha\ing  continuous  l'(xj)  and  satis- 

f\ine  the  followino  conditions: 


1.  V(\.t)  is  positixc  definite,  and 

2.  /'(.v.r)  is  negatixe  definite, 


then  the  equilibrium  state  at  the  origin  is  asymptotically  stable.  y'{-x,t)  is 
negative  definite  which  shows  that  V(x,t)  is  continually  decreasing.  So  for  any 
svstem 


-v=/(x,«(r))  (2.4) 


where,  x  is  the  state  variable 
u(t)  is  the  control  law 
x(0,t)=A-o 

and  the  time  derivati\e  of  the  Liapunov  function  V{x,u{t))  is  negative  definite, 
then  control  law  u(t)  is  guaranteed  stable. 

C.     DYNAMICS  OF  SYSTEM  WITH  SWITCHING 

Now  consider  the  case  that  an  asymptotically  stable  system  may  consist  of 
two  structures  neither  of  which  is  asymptotically  stable.  If  the  differential 
equations  of  the  second  order  system  have  the  following  format: 


dt 

dx2 
IT 


Ux,  (2.5) 


where        u  is  a  constant  <  |  4^'  | 

then  the  structure  of  the  system  is  elliptic  as  described  in  Figure  1  on  page  6. 
Suppose  the  system  with  a  positi\e  feedback  gain,  then  structure  is  aperiodically 
unstable  as  shown  in  Figure  1.  The  block  diagram  of  the  closed  loop  system  with 
switching  gain  is  illustrated  in  Figure  2  on  page  7.  Let  us  try  to  combine  the 
ad\antage  of  both  systems  by  suitable  choice  oi'  their  structures  in  the  appropri- 


Figure   1.      Asymptotically  unstable  structure  (Ref.5] 

ate  parts  of  the  phase  plane.  In  order  to  get  the  asymptoticall}'  stable  structures 
in  Figure  3  (b),  the  phase  plane  was  divided  into  four  pairwise  subsections  as 
shown  in  Figure  3  (a)  by  the  following  conditions: 


1. 

Subsection 

I 

2 

Subsection 

11 

3. 

Subsection 

111 

4. 

Subsection 

IV 

A",  >  0,      A-^  +  M'.v,  >  0 

A",  <  0,        A-2  +  M^A-,  >  0 
A|  <  0,        A2  +  M'a,  <  0 

A,  >0,      a^+Ta,  <0 

The  good  phase  trajectory  for  each  phase  portrait  has  been  chosen  to  make 
asymptotically  stable  structures  as  in  Figure  3  (b). 

This  phase  plane  is  separated  from  one  another  by  the  straight  line  a,  =0  and 
A.+M^v,  =0  which   we  call   the   switch   line  or  sliding  surface.   The   asymptote 


Fij^ure  2.      The  block  diagram  ^^ith  snitching  gain 


A"2+M'.\',  =0  acts  as  a  switching  line  for  the  structure  when  the  trajectory  of  the 
subsection  I  is  reached.  The  structure  of  the  system  can  be  switched  by  using  this 
line  instantaneously  from  elliptic  to  hyperbolic  in  this  case.  This  switching  line  is 
\ery  important  in  sliding  mode  control.  Once  the  system  slate  trajectory  ap- 
proaches the  switching  line,  and  in  order  to  keep  the  trajectory  on  the  sliding 
surface  for  t  >  r^,  then  this  system  will  become  asymptotically  stable  as  long  as  it 
satisfies  the  Liapunov  condition.  In  the  general  case,  a  switching  line  might  be  a 
straight  line,  o  =  ^"2+^X3  (0  <  A  <  00),  but  must  pass  through  the  origin  (i  e 

C7(0)-0). 

D.     SLIDING  CONDITION 

Some  of  the  possible  advantages  offered  by  the  idea  of  switching  the  structure 
of  a  control  system  were  described  in  the  last  subsection.  But  we  remarked  that 
if  the  structure  does  not  change  at  the  precise  instance  when  the  trajectory  crosses 
the  switching  line,  due  to  the  effect  noise,  then  additional  control  action  will  be 


Figure  3.      Phase  plane  division  and  combined  stable  structure  (Ref.S] 


required  lo  enforce  ihe  sliding  condition.  The  motion  of  the  system  now  depends 
on  shding  surface  parameters  which  are  insensitive  to  the  external  disturbances 
and  \ariations  of  the  plant  parameters  within  a  wide  range  of  the  switching  line. 
If  the  system  trajectory  of  the  subsection  I  crosses  into  the  switching  line  and  af- 
ter passing  the  system  trajectory  of  the  subsection  II  crosses  oxer  the  switching 
line  repeatedly  again,  the  system  trajectory  will  be  kept  within  some  range  of  the 
switching  line.  Suppose  such  change  occurs  at  infinitely  high  frequency,  then  the 
state  of  the  system  trajectory  is  maintained  with  an  infinitesimal  amplitude  oscil- 
lation. It  is  an  asymptotically  stable  system  on  this  line  as  shown  in  Figure  4  on 
page  9. 

The  motion  of  the  system  on  the  switching  line  is  described  by  the  solution 
of  the  general  differential  equation  (2.5)  [Ref.  2]  together  with  the  equation  of  the 
general  switching  line  gi\en  by 


1X2 

\                                                                                               ^\^ 

\                                                                                                            \ 

« 

\ 

\ 

\ 

\ 

Figure  4.      Snitching  line  .ind  asymptotic  trajectory  [Ref.5] 


^(A-)  =  (-^+;.r'-v  =  () 


(2.6) 


where       x  =  -x— .v^,  a  single  element  of  tiie  state  vector 
i  >  0  (  arbitrary  constant) 
n  =  order  of  svsteni 


The  motion  defined  by  the  equation  (2.6)  describes  the  system  dynamics  in  tlic 
sliding  mode.  A  sliding  mode  has  an  important  property  that  the  corresponding 
motion  of  the  system  depends  on  the  sliding  surface  (Switching  Line)  which  is 
chosen  by  the  designer  only.  In  order  to  know  the  mathematical  existence  condi- 


lion  for  the  sliding  mode,  the  tlieory  of  sliding  mode  has  gi\en  a  considerable  at- 
tention to  the  methods  guaranteeing  the  existence  of  the  sliding  mode. 
1.     Condition  for  Existence  of  a  Sliding  Mode 
Considering  the  general  dynamic  system 


"^''^  =yi^,o  (2.7) 


dt 


Let  us  assume  that  the  right  hand  members  of  this  equation  are  discontinuous 
on  a  certain  sliding  surface  g{x)  =  0  in  the  phase  space,  where,  as  phase  trajectory 
of  the  system  approaches  g{.x)  =  0  from  either  side,  the  following  limits  are  de- 
fined [Ref.  5]: 


Hm  J{xj)=r{-^'^r) 

(T-O 


lim  Av.O=y''(AV)  (2.8) 

Where      f^{xj)  ^f'{x,t) 

then  the  derivative  of  the  sliding  surface  [a]  along  the  trajectories  of  the  system 
is 

d(7        ca    dx        CO   r     tr        ^  \  /">  c\\ 

at        ex    at        ex 

where       f  =  phase  \elocity  \ector,  and 


lim   -^  =  (/"g/vzt/a) 

(T-  0     dt 
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da 


Hm   ^=  if  grade)  (2.10) 

-    dt 


where       o,f  is  a  smooth  function. 

At  each  point  of  a  =  0,  the  sign  of  the  limit  equation  (2.8)  has  seven  cases.  There 
is  a  case  which  is  most  interesting  as  it  corresponds  to  the  existence  of  an  ideal 
sliding  mode  on  the  sliding  surface  a  =  0,  if 

lim  -^<0        .■ 

lim  4^>0  (2.11) 

<T^o-  dt 

The  equi\'alcnt  inequality  being  the  condition  for  existence  of  a  sliding  mode  is 

lima4-<0 

C7-*0  dt 

or 

dio'^) 
lim— ^ — <0  2.12 

(T-o     dt 

2.     Proof  of  Stability 

This  inequality  is  also  suggested  by  [Rcf.  5]  as  a  necessary  condition  for 
the  system  in  equation  (2.5)  to  have  a  degenerating  Liapunov  function  in  the 
following  form: 


V{x)  =  -^[a{x)f  (2.13) 
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The  asymptotic  stability  of  the  system  (2.5)  is  guaranteed  provided  that  l'{x)  is 
negati\e  definite  as  discussed  in  the  previous  section.  If  cj{xj)  is  a  sHding  surface 
by  the  definition  abo\e.  it  follows  that 

-^-^o\A-j)<-ir\o[.x,t)\  (2.14) 


where        ?/  is  the  sliding  control  gain  and 
G{x,t)  is  the  sliding  surface. 

It  will  guarantee  stability  of  the  sliding  mode  motion.  The  control  law  dri\ing 
motion  in  the  sliding  mode  can  be  obtained  by  using  this  condition  [Ref.  2].  The 
next  section  will  be  de\otcd  to  a  discussion  for  the  development  of  the  sliding 
mode  control  law. 

E.     SLIDING  SURFACE  DESIGN 

The  sliding  surface  has  a  \cry  important  property  that  is  shown  in  the  previ- 
ous section.  The  sliding  surface  should  be  designed  so  that  system  response  re- 
stricted to  f7(A-)  has  a  desired  beha\ior.  such  as  asymptotically  stable  state  or 
tracking  error. 
Let  us  consider  a  linear  time  invariant  system  to  design  the  sliding  surface 

x  =  Ax+Bu  (2.15) 

where    .v  e  R\    u  e  R"" 


Consider  that  the  sliding  surface  of  the  equation  (2.15)  has  the  following  form: 

(7(a-)  =  S^a-  =  0  (2.16) 


where     S  is  the  sliding  surface  coefficient  [in  x  n) 
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The  existence  of  the  shding  mode  imphes  that  (j{x)(7{x)  <  0  and  c{x)  =  0  for  all 
t  >  tQ.  Using  the  method  of  equivalent  control  [Ref.  6] 

d{x)  =  0  =  4^4^  =  S^x  =  0  (2.17) 

ex    at 


Substituting  equation  (2.15)  for  x  of  the  abo\e  equation 


or 


^eqi 


u^^  =  -ls'^BT^S^Ax  (2.18) 


Substituting  equation  (2.18)  to  Eq  (2.15)  and  rearranging 

x  =  Ax-Bls'^BT^S^Ax 


or 


X  =  [A-B(S'''Br^S^A^x  (2.19) 


Equation  (2.19)  gi\es  the  dynamics  of  the  system  on  the  sliding  surface  for  t  >  tQ 
gi\en  a(x)  =  0,  but  the  S  matrix  is  unknown.  In  order  to  determine  S  matrix,  the 
equation  (2.19)  can  be  rearranged  in  the  following  form: 

x  =  \_A-BKJx 

X  =  A,x  (2.20) 

where  A',  =  {S'^B)''SL4 

A,  =  A-BK, 

The  K.  matrix  can  be  obtained  from  the  pole  placement  for  which  we  can  select 
specifically  desired  closed-loop  poles  of  the  s\slcm  equation  (2.20)  on  the  sliding 


13 


surface.  If  \vc  get  the  A',  matrix  b}'  using  the  standard  pole  placement  method,  the 
sliding  surface  matrix  (m  x  n)  can  be  determined  in  the  following  procedure: 

S^{A-BIQ  =  S'^A,  =  0  (2.21) 


It  should  be  noted  that  A^  must  be  rank  deficient  by  one  and  that  the  procedure 
must  therefore  place  one  pole  of  A^  at  the  origin.  The  left  eigenvector  of  the  A, 
matrix  of  equation  (2.21)  corresponding  to  a  pole  placed  at  the  origin  are  the 
sliding  surface  coefficients  which  give  the  desired  beha\ior  system  on  the  sliding 
surface, 

F.     SLIDING  MODE  CONTROL  LAW 

Gi\en  the  dynamic  model,  the  sliding  surface  definition,  and  the  stabihty 
criteria,  a  suitable  control  law  can  be  obtained.  We  assume  that  a  wide  range  of 
single  input,  single  output  dynamic  systems,  and  sliding  surfaces  can  be  described 
by 


A-  =  Ax + Bit  (2.22) 


(j(a-,0  =  S^a-(/) 


where  S^  is  a  row  x'ector  of  the  form  [1,  5.,  53....5„_,],  a  specific  choice  of  5^  to 
achic\"c  a  stable  tracking  error  and  to  enhance  robustness  as  discussed  in  the 
pre\ious  section.  If  ii{t)  could  be  chosen  so  as  to  keep  the  trajectory  on 
cr(.v.r)  =  0,  we  would  ha\e  the  sliding  control  law  from  the  sliding  condition 
equation  (2.14)  and  sliding  surface  equation  (2.16)  that 
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T  1 

d{x)  =  S  X  =  —f]  sign{(7) 


S^A+S^Bu  =  -nlsign{o) 
u  =  -{s'^B)~^s'^Ax-{S^Bf^r}lsign{a)  (2.23) 

where       r]l  is  a  arbitrary  nonlinear  feedback  gain 

The  control  law  has  two  parts. 

u  =  u+u 

where  u  is  linear  feedback  control  law 

u  is  nonlinear  feedback  control  law 

Initially,  u  compensates  directly  for  the  known  portions  of  the  dynamics.  Thus, 
u  is  discontinuous  across  the  sliding  surface.  This  nonlinear  term  is  obtained  di- 
rectly from  the  time-\'arying  bounds  on  parametric  uncertainty  and  disturbances. 
As  a  result,  control  discontinuity  across  (7=0  grows  as  the  model  becomes  less 
certain  and  increasingly  disturbed.  This  insures  that  cr^  is  a  Liapuno\"  function 
of  the  closed-loop  system,  since  it  satisfies  the  sliding  condition  equation  (2.14) 
and  thus  guarantees  stability  despite  the  uncertainty  in  the  model  and  disturb- 
ances [Ref.  2].  This  type  of  control  law  can  guarantee  stability  and  perfect 
tracking  for  a  large  class  of  nonlinear  systems.  The  discontinuous  form  results  in 
a  chattering  type  of  control  action  that  would  be  \'ery  undesirable  for  most  sys- 
tems and  this  chattering  beha\ior  has  been  one  of  the  main  reasons  sliding  con- 
trol techniques  ha\'e  not  been  more  widely  applied.  This  problem  will  be  solved 
by  smoothing  out  the  control  law  in  a  thin  boundary  layer  around  the  sliding 
surface  as  2i\'en  in  the  next  section. 


15 


G.     CONSIDERING  CHATTERING  AND  UNCERTAINTY 

While  sliding  mode  control  provides  a  control  law  which  is  robust  to  param- 
eter variations  and  disturbance  inputs,  it  was  a  chattering  problem  for  the  input 
as  shown  in  Figure  5  on  page  17.  In  fact,  imperfections  such  as  delays  in 
switching  and  hysteresis  in  switching,  will  cause  the  trajectory  to  chatter  along  the 
sliding  surface.  Although  as  such  imperfections  vanish,  control  acti\'ity  remains 
as  undesirable  switching  and  high  frequency  signals  on  the  sliding  surface. 

The  basic  idea  of  eliminating  chattering  is  simple.  This  chattering  problem 
caused  by  a  discontinuous  and  nonlinear  switching  feedback  control  law  can  be 
eliminated  by  replacing  it  with  a  continuous  feedback  control  law.  But  if  the 
sliding  mode  control  has  a  continuous  feedback  function,  there  are  steady  state 
errors  due  to  \ariations  in  parameter  and  disturbance  [Ref.  2].  Suppose  the  con- 
trol law  has  a  continuous  feedback,  whose  terms  are  continuous  function  inside 
a  small  boundary  layer  thickness  on  the  sliding  surface,  as  shown  in  Figure  6  on 
page  18.  then  the  steady  state  error  can  be  calculated  by  a  smooth  function  which 
eliminates  chattering  in  the  boundary  layer  thickness  ((/>).  This  boundary  layer 
thickness  can  be  determined  directly  from  the  desired  sliding  surface  coefficient 
limit  and  estimates  of  the  uncertainty  dynamics  of  the  s^'stem  to  be  controller.  If 
the  specified  bounds  on  disturbances  and  parameter  uncertainty  are  not  ex- 
ceeded, the  system  is  guaranteed  to  stay  within  the  boundary  la\'er  once  inside. 
If  a  disturbance  temporarily  exceeds  the  specified  bounds,  the  state  may  go  out- 
side the  boundary  layer.  Howe\"er.  the  sliding  condition  equation(2.I4)  implies 
that  the  system  will  alwa\'s  mo\"e  back  inside  the  boundary  layer  once  the  dis- 
turbances return  to  their  projected  le\'els. 

The  dynamics  of  the  state  trajectory  inside  the  boundary  are  only  an  ap- 
proximation to  the  desired  dynamics  on  the  sliding  surface.  The  advantage  of  the 
scheme  is  that  the  state  trajectory  does  not  chatter  close  to  the  sliding  surface. 
To  carry  out  the  preceding  program,  we  use  the  sliding  surface  considered  in  the 
pre\  ious  section  with  o{xj)  of  the  form: 

a{x)  =  /,v(0  (2.24) 
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Figure  5.      Chattering  problem 

where       x{t)  =  x{t)-xXt) 

Xj^t)  =  desired  state  variable 

To  define  the  boundary  layer  thiekness  about  the  sliding  surface  of  equation 
(2.14).  define 

o^{x)  =  a{x)+(}) 


a    (a)  =  o{x)-(f) 


(2.25) 


where       4^  is  the  boundary  layer  thickness 


It  is  immediately  from  equation  (2.25)  that 
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Figure  6.      Boundary  layer  thickness  and  saturation  function 


I  ^(.v)  I  <  4> 


(2.26) 


and 


■io-M 


ill  cll 


(2.27) 


We    choose    the    control    law    u{()    as    by    equation    (2.27)    for    a'{x,[)  <  0    or 
o-(a',0  >  0.  This  guarantees  that 


dt 


o~{x,t)  >  0 


(2.28) 


-^c7+(.x-,r)<0 

cll 


(2.29) 


Equation  (2.28)  and  (2.29)  establish  that  trajectories  starting  outside  boundary 
layer  tend  towards  boundary  layer,  and  further  trajectories  starting  inside 
boundary  layer  stay  in  it  for  t^,  <  t.  It  only  remains  to  specify  u{.\',t)  to  be  a  con- 
tinuous function  of  x  inside  boundary  layer  thickness. 
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III.     SLIDING  MODE  CONTROL  FOR  NONLINEAR  A.U.V.  IN  DIVE 

PLANE 

A.  GENERAL 

Underwater  vehicles  present  difficult  control  system  design  problems  due  to 
their  nonlinear  dynamics,  uncertain  hydrodynamic  coefficients  and  the  presence 
of  disturbances  that  are  difficult  to  measure  or  estimate.  This  chapter  describes 
the  dynamics  of  a  selected  automonous  underwater  vehicle  (AUV)  and  the  design 
of  a  sliding  mode  controller  which  can  handle  these  problems  effectively. 

Motions  of  underwater  vehicles  are  expressed  in  a  body  fixed  reference  frame, 
because  hydrodynamic  forces  and  inertia  properties  are  most  readily  computed 
in  a  ship  reference  frame.  The  nonlinear  equations  of  motion  in  six  degrees  of 
freedom  vehicles  which  are  commonly  known  as  the  DTNSRDC  2510  equations 
of  motion  are  used  for  verification  of  the  sliding  mode  control  design.  These 
highly  nonlinear  equations  of  motion  are  linearized  by  a  Taylor  series  expansion 
and  modified  to  suit  the  needs  of  an  AUV  [Rcf.  7].  First,  these  linearized  and 
modified  equations  of  the  system  are  used  to  design  the  sliding  surface,  sliding 
mode  control  law  and  obscr\er.  These  \alues  of  the  linear  system  are,  then,  used 
to  implement  the  sliding  mode  control  law  for  the  nonlinear  system.  This  chapter 
shows  how  to  design  the  sliding  mode  control  for  the  highly  nonlinear  AUV. 

B.  NONLINEAR  COMPUTER  MODEL 

The  nonlinear  model  used  for  sliding  mode  control  verification  was  deri\ed 
from  the  original  NSRDC  2510  document  [Ref.  8].  The  nonlinear  model  used  in 
this  thesis  consists  of  8  differential  equations  which  describe  the  AUV  dynamics. 
The  six  equations  as  derived  from  force  and  moment  equalities  account  for  the 
states  u,  \-.  w.  p.  q  and  r.  The  shape  of  AUV.  which  is  17.4  feet  long,  weighs 
12000  pounds,  and  neutrally  buoyant,  is  depicted  in  Figure  7  on  page  21. 
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Figure  7.      Sketch  of  tlie  autonomous  undenvater  vehicle 
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General  dynamical  equations  are  derived  from  Newton's  law  in  an  inertial 
reference  frame: 

\  ^>  =/[  dynamical  response  terms  )  (3.1) 

The  general  form  of  the  force  balance  is 

?  =  ^  [Mx]  (3.2) 

at 


where        M  =  mass  matrix 
A-  =  [  w,  V,  w  Y 


and  the  moment  balance  is 


.U  =  ^{IQ)  (3.3) 


where        I  =  moment  of  inertia  matrix 
Q  =  lp.q.rY 

Three  dimensional  motions  of  underwater  vehicles  are  normally  described  using 
the  body-fixed  coordinate  and  inertial  reference  frame.  Position  of  the  body-fixed 
coordinate  system  is  expressed  in  X.  \'.  and  Z  coordinates  and  orientation  of  the 
\ehiclc's  coordinate  system  in  expressed  in  Eulcr  angles  (p.  0.  and  ip.  The  defi- 
nitions of  u.  V.  w,  p.  q.  r  and  controls  are  listed  in  Table  1  and  Table  2. 
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Table   1. 


DEFINITION  OF  A  IJ  V  STATES. 


STATE 

DEFINITION 

UNITS 

u 

surge  rate 

(ft  s) 

\- 

sway  rale 

(fts) 

\v 

hea\e  rale 

(fts) 

p 

roll  rate 

(rad  s) 

q 

pitch  rate 

(rad  s) 

r 

yaw  rate 

(rad.s) 

0 

spin 

(radians) 

0 

elexator 

(radians) 

0 

azimuth 

(radians) 

Table  2.       DEFINITION  OF  A.U.V  CONTROLS 


CONTROL 

DEFINITION 

i\ 

rudder  angle 

^\. 

starboard  bow  plane  angle 

i\. 

port  bow  plane  angle 

<)  _ 

stern  plane  angle 

^\. 

delta  form 

<)'. 

delta  buoyancy 

The  dynamical  response  terms  of  the  left  hand  side  of  equation  (3.1)  or 
equation  (3.2)  and  (3.3)  express  the  external  forces  and  moments  exerted  on  the 
vehicle  by  hydrodynamic.  control  surface,  propulsion  and  other  effects.  The  force 
and  moment  equalities  of  equation  (3.2)  and  (3.3)  describe  motions  in  six  degrees 
of  freedom  of  the  AUV.  The  three  forces  are  in  the  axial,  lateral  and  normal  di- 
rections which  gi\e  rise  to  motions  in  surge,  sway  and  hca\e  respectively.  The 
three  moment  equations  produce  moments  and  motions  in  roll,  pitch  and  yaw. 
Figure  8  on  page  24  shows  the  positi\e  directions  of  forces,  moments,  motions, 
and  control  surface  dellections. 
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Figure  8.      Positive  motion  directions  of  the  AUV 


The  equations  of  motion  for  the  six  degrees  of  freedom  for  the  fully  nonlinear 
model  are  listed  in  the  following  page.  The  hydrodynamics  coefficient  of  those 
equation  used  for  this  thesis  are  those  that  were  determind  using  an  analytic  ap- 
proach [Ref.  7]  and  later  simplified  [Ref.  9].  The  four  nonlinear  equations  that 
are  considered  for  designing  sliding  mode  control  of  the  AUV  arc  written  in  the 
following  form: 


Normal  Equation  of  Motion 


miiv-u^q+yp+XApr-q)-^  yjqr+p)-ZJj)  +q  )] 
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—  ["^[Z'  ^q+Z'  py^+Zp,pr+Z'  .y^ 


+  —  l'\_Z'^M'+Z'qU^q+Z\,pYp+Z\.,vr^ 


+  -f  ^'[^'H-WxVV+ZV,v2+w;(Z'^,(5,+Z'^^d^)] 


+  {IV-B)cos0cos(t) 


+  y  ^'^'qn^'x(I^{^])+  Y  ^^(Z'vv„''x^^'+2'^,,,w^^,)£;(?7) 


(3.4) 


Pitch  Equation  of  Motion 


Iyq+{I^-I,)py-I   {pq-y}+I^,{p^-r^)-mlX  {w-u^q+vp)-Z Ju-vr+wq)^ 


J^lX\r^q+M'^y~+M'prpr+M'nr'^ 


+  4-  l\M\^v+^r,u,q+M\,pVp+^I\r^■r■] 


+  -T  l\^f\,u,^^■+^ry+ul{^fs^5,+M's,s,):\ 


[Cp,M.v)(v  +  A-/f+Coi'(.x)(u-X^)-]  -TT-fy-  A7/A- 

L'cy\Aj 


2!) 


-{XJV-Xj^B)  cos  e  COS  4)-{Z  IV-ZsB)  sin  6 


+  -f  /'A/'^^z/,./£(/,j+  4  /■\a/'.-,".^'+^/'6.,"'^.)^('/)  (3-5) 


Kinematic  Relations 


0  =  q  cos  (p—rs'm  (f)  (3.6) 

Z  =  — z/^  sin  d+v  cos  ^  sin  (jy+w  cos  0  cos  </>  (3.7) 

The  simulations  for  the  di\e  plane  control  were  performed  by  using  the 
FORTRAN  language  code  (Appendix.  B)  for  the  simulation  of  nonlinear  system 
response  as  a  function  of  time. 

C.     LINEAR  MODELING 
1.     Equations  of  Motion 

The  sliding  mode  controller  design  procedure  begins  with  the  expression 
of  the  equations  of  motion  in  linear  time  invariant  state  space  form.  The  highly 
nonlinear  AUV  system  is 

-^  x{t)  =  M-'AAth  uiO)  (3.8) 

at 

3'(0  =  giAO)  (3.9) 

where        x  is   the  state  \ector 

u   is   the  control  input  \'ector 
y   is  the  output  \ector 

Although  equations  (3.8)  can  be  significantly  simplified  as  in  Larsen  [Ref.  9],  they 
appear  still  \ery  complex  for  this  study.  The  nonlinear  equations  can  be  linearized 
through  a  Taylor  scries  expansion  in  the  \  icinity  o[  a  nominal  point  (ideally  where 
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d_ 

dt 


[t]  =  0)  for  small  deviations  of  u(t)  and  x(t)  from  the  reference  values.  These 


equations  of  motion  were  linearized  by  Boncal  [Ref.  7].  Alternatively,  least 
squares  techniques  can  be  used  for  parameter  identification  in  order  to  develop 
a  hnear  model  for  the  relationship  between  dive  plane  angle  (S).  pitch  rate  (q), 
depth  (z)  and  pitch  angle  (6)  [Ref.  10].  The  linearized  and  \ery  simplified 
equations  for  the  dive  plane  motion 

q  =  -0.7(7-0.030-0.0355 


U^d 


(3.10) 


were  found  to  pro\  ide  a  satisfactory  approximation  of  the  open  loop  dive  plane 
dynamics  oi^  nonlinear  equation  (3.4)  to  (3.7)  for  the  nominal  speed  of  6  ft/sec  (or 
500  rpm  ). 

2.     Sliding  Surface  Design 

It  is  e\ident  from  the  discussion  of  the  sliding  mode  theory  (Chapter  2) 
that  description  of  AUV  motion  depends  on  the  sliding  surface  regardless  of  dis- 
turbance and  unmodel  parameters,  after  it  hit  sliding  surface.  So  it  is  very  im- 
portant to  design  sliding  surface  of  the  AUV  with  disturbance  and  unmodelled 
parameters.  The  sliding  surface  of  the  AUV  will  be  designed  based  on  the  linear 
model.   The  state  space  form  of  the  linear  model  is 

X  =  Ax+Bu 


V  =  Cx 


(3.11) 


where 


A-^  = 

[q.  e.  z] 

"-0.7    -0.03  0 

A  = 

1           0       0 

0         -U,    0 
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B  = 


■0.035 
0 
0 


and 


u  =  d 


C=  [00  1] 


For  a  three  dimensional  system,  the  sliding  surface  is  the   Euclidean  plane 


where      5  = 


Equation  (2.6)  can  be  expressed  in  terms  of  the  state  \ariables  and  sliding  surface 
coefficients  as  followine 


a{x)  =  q+S20+S}Z 


(3.12) 


where  the  coefficient  of  q  has  been  normalized  to  1.  S  will  determine  the  sliding 
surface  plane  uniquely.  To  compute  the  equivalent  control  (u^^),  wc  substitute  A, 
B  and  S  in  to  equation  (3.1 1).  and  the  closed-loop  dynamics  of  the  linear  model 
are 


x  =  [A-BiS'^Bf^s'^Alx 


or 


x  =  (A-BK)x 


(3.13) 
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where  the  gain  \ector  K  can  be  found  from  standard  pole  placement  methods. 
The  closed-loop  dynamics  matrix 

A,  =  A-BK  ,  (3.14) 

where     K  =  (5^5)  'S^/4 

has  eigenvalues  specified  for  desirable  response  of  the  AUV.  One  of  the 
eigenvalues  of  A,  must  be  specified  as  zero.  With  A^  specified  and  K  computed 
from  pole  placement,  S^  matrix  can  be  determined  as  using  the  equation  (3.13) 
and  (3.14). 

S^(.4-5A>0 

or 

S'^A^.  =  0  (3.15) 

Therefore  5^  is  found  as  a  left  annihilator  of  A,  or  S  is  a  left  eigenvector  of  A^ 
which  corresponds  to  the  zero  \aluc.  This  sliding  surface  of  the  AUV  satisfy  the 
sliding  surface  condition  S^x  =  0.  To  find  proper  sliding  surface  of  the  AUV, 
Matrix-x  program  "SCM"  in  Appendix  A  is  used.  The  response  of  the  AUV  ac- 
cording to  different  sliding  surface  is  shown  in  Figure  9  on  page  30,  Figure  10 
on  page  31.  and  Figure  11  on  page  32.  The  sliding  surface  of  Figure  10  which 
has  no  overshot  and  fast  response  time  is  selected  as  the  reference  sliding  surface 
of  the  AUV  in  order  to  design  control  law.  The  sliding  surface  of  the  AUV  is 

a{x)  =  ^+0.520-0.01 12z  (3.16) 

This  sliding  surface  has  the  desired  dynamics  of  the  closed-loop  system.  The 
perfect  depth  tracking  of  the  selected  AUV  is  then  defined  as  remaining  along  the 
surface.  The  dynamic  response  of  AUV  is  affected  by  the  chosen  sliding  surface. 
The  selected  sliding  surface  is  used  to  handle  accurate  depth  control. 
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Fi<;ure  9.      The  dyiianiic  response  for  the  closed-loop  poles  =  0,-0.1,-0.15 
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31 


0.0      30.0       60.0       90.0 

TIMEISEC) 


120  0.0      30.0       60.0       90.0 

TIME(5EC] 


120 


30.0       60.0       90.0 

TIME (SEC) 


o 

CD 

CxJ 

cn  o 

\   rv 

□ 

cn 

--'    o 

C'J 

cn 
lt:  o 


cn 

CD 

O 

-J 
cn 


0.0      30.0       50.0       90.0 

TIME(5EC] 


120 


Figure   II.      The  dynamic  response  for  the  closed-loop  poles  =  0,-0.45,-0. 47 
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3.     Sliding  Mode  Control  Law 

Here  the  goal  is  to  determine  switched  feedback  gains  which  will  dri\e  the 
AUV  state  trajectory  to  the  sliding  surface  and  maintain  sliding  mode  conditions. 
By  defining  the  Liapunov  function  of  the  equation  (2.13j 

J/(^)  =  i-  [C7(A-)]-  (3.17) 

where        (7(x)  =  ^+0.520-0.01 12z 

asymptotic  stability  of  the  AUV  on  the  sliding  surface  is  guaranteed,  that  pro- 
vided l'{x)  is  a  negatixe  definite  function  or 

(7{x)c{x)  =  -}]^  I  (7{x)  I 


or 


d(x)  = -?]\^ign(a{x))  (3.18) 


Since  o{x)  =  5^x,  we  have 


s'^{Ax+Bu)  =  -}]^sign(G(xj)  (3.19) 


and  soh  ing  for  the  equivalent  control  input  u 

u  =  -(S'^Br\s'^Ax-iS^B)~\]^sign(G)  (3.20) 

It  is  important  to  recognize  that  the  feedback  control  law  is  composed  of  two 
parts, 

u  —  u+u 

The  first  u  =  -{S'^By-S'^Ax  is  a  linear  feedback  control  law,  where  the  second 
U  =  -{S^B}'hfsign(G{x))  is  a  nonlinear  feedback  with  its  sign  toggling  between 
plus  and  minus  according  to  which  side  of  the  sliding  plane  the  AUV  is  located 
in.  Two  comments  are  in  order  here:    First,  since  u  has  to  change  its  sign  as  the 
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AUV  crosses  o{x)  =  0,  the  sliding  surface  has  to  be  a  hypeiplanc  (dimension  of 
one  less  than  the  state  variable).  Second,  it  is  ?7  which  is  main!}'  responsible  for 
driving  and  keeping  the  AUV  onto  the  sliding  plane  a{x)  =  {)  (whcie  77  =  0  as 
well).  The  block  diagram  of  the  sliding  mode  control  is  illustrated  in  Figure  12 
on  page  34. 


Linear  gain 


u 


r® 


B 


d(t) 

Disturbance 


<X)-<X 

+T 


U 


PLANT 


n 


Outp^jt 


11 


-d^ 


+1 


Nonlinear  gain  Switching  logic 


Figure   12.      The  block  diagram  of  the  sliding  mode  control 

Pro\ided  that  the  gain  has  been  chosen  large  enough,  JJ  can  pio\ide  the 
required  robustness  due  to  momentary  disturbance  and  unmodeled  AUV  without 
any  compromise  in  stability.  The  linear  feedback  law  is  designed  such  that  the 
AUV  has  the  desired  dynamics  on  the  sliding  plane.  Since  a{.\)  =  0  in  this  case 
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u  =  u  =  -{s'^B)~^s'^Ax 


(3.21) 


Large  enough  gain  (ETA  =  ?/^(5^5)-' =4.0)  could  be  used  in  the  computer  simu- 
lation by  using  Matrix-x  and  FORTRAN  program  "SMC"  in  Appendix  A.  The 
dynamic  response  of  the  AUV  according  to  several  values  of  gain  is  shown  in 
Figure  13  on  page  37,  Figure  14  on  page  38,  and  Figure  15  on  page  39.  Choos- 
ing properly  large  enough  nonlinear  feedback  gain,  although  the  response  time 
of  the  AUV  is  fast,  the  AUV  overshoots  on  the  sliding  surface  as  shown  in 
Figure  15  on  page  39.  The  dive  plane  angle  of  the  AUV  presents  chattering  due 
to  sign  switching  of  the  nonlinear  part  of  the  control  law.  Several  varieties  of 
nonlinear  feedback  gain  (?/-)  and  boundary  layer  (0)  are  evaluated  through  a  se- 
ries of  numerical  experiments  in  order  to  analysia  dynamic  response  of  AUV 
which  are  dependant  on  it.  The  characteristic  of  dynamic  response  according  to 
the  nonlinear  feedback  gain,  boundary  layer,  and  closed-loop  poles  are  shown  in 
Table  3. 


Table  3.       THE  DYNAMIC  CHARACTERISTIC 
TO  DESIGN  PARAMETER  s 


OF  THE  Am'  ACCORDING 


Parameters 

Depth  re- 
sponse 

Di\e  plane 
angle 

Pitch  angle 
(radians) 

Sliding  sur- 
face 

C.P  = 

0.-0.1.-0.15 

slow,  no 
o\ershoot 

no  saturated 

maxi 
angle  =  0.4 

no  o\  ershoot 

C.P  = 

0.-0.25.-0.27 

fast,  no  o\er- 
shoot 

no  saturated 

maxi 
angle  =  0.41 

no  oxcrshoot 

C.P  = 

0.-0.45.-0.47 

fast,  o\er- 
shoot 

saturated 

maxi 
angle  =  0.42 

overshoot 

ETA  =  3. 
PHI=0.2 

slow.  o\er- 
shoot 

no  saturated 

maxi 
angle  =  0.4 

oxershoot 

ETA  =  4. 
PHI  =  0.4 

fast,  no  o\er- 
shoot 

no  saturated 

maxi 
angle  =  0.41 

no  oxershoot 

ETA  =  8. 
PHI=0.4 

fast,  no  oxer- 
shoot 

saturated 

maxi 
ancle  =  0.42 

oxershoot 
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We  already  knew  that  system  behavior  is  dependent  on  the  shding  surface  which 
is  designed  based  on  closed-loop  poles.  The  closed-loop  poles  (0,-0.25,-0.27), 
nonlinear  feedback  gain  (ETA  =  4),  and  the  boundary  layer  ((})  =  0.4)  are  selected 
in  order  to  satisfy  the  necessary  motions  of  the  AUV. 
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4.     Cliatleiing  Problem  and  Steady  State  Error 

The  nonlinear  part  of  the  sliding  mode  control  may  give  rise  to  chattering 
where  constant  or  random  disturbances  are  present,  as  shown  in  Figure   16. 
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Figure   16.      The  chattering  problem  of  sliding  mode  control 

A  choice  of  a  saturation  function  (satsgn((7))  instead  of  the  pure  switch  (signfrr)) 
for  the  nonlinear  control  law  is  prcfcrcd  in  order  to  a\oid  chattel  ing  pioblcins. 
Since  the  boundary  layer  thickness  is  related  to  the  characteristic  of  dynamic  re- 
sponse, it  can  be  selected  according  to  the  manu\cring  conditions  of  underwater 
\  chicles.   Therefore  the  control  law  for  the  AUV  is  modified  to 


u  =  -5  A429q+\. 07  \AO  +  K,^satsgn(a) 


(3.22) 


where        A;  =  ;/'(5^Z?)-' 


and 
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+  1  if    G>4) 

satsgn{G)  —    —\  if    a  <  —4>  ' 

-^  //-(/)<  (7  <(/) 

This  proN'ides  a  linear  interpolation  across  the  boundary  layer  as  illustrated  in 
Figure   17  on  page  42. 

A  boundary  layer  thickness  ((/))  essentially  assigns  a  lowpass  filter  structure  to  the 
dynamics  of  the  sliding  surface  [a]  .  If  a  specific  bound  on  disturbance  is  not  ex- 
ceeded, the  S3'stem  is  guaranteed  to  stay  within  the  boundary  layer  once  inside. 
The  steady  state  error  of  the  sliding  mode  control  of  the  AUV  with  a  disturbance 
can  be  computed  as  following.  From  the  equation  (3.1 1) 

q  =  -0.7q-0.03>d-0.035u+d  (3.24) 

where        d   is   disturbance 
q  =  q^O 
6  —  0    in  steady  state 

The  steady  state  control  input  for  the  gi\en  disturbance  is 

"'«=a^  (3.25) 

The  control  law  in  steadv  state  is 


(P 


u  \^^  —  ii+u  =  }fsatsgn{  -7- )  (3.26) 


where  a{x)  = -0.0\\2e, 
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Figure   17.      The  saturation  function  for  the  nonhnear  control  law. 


"  \ss  =  '/   ( 


2    -0.01 12  xc^ 


</> 


(3.27) 


e.  =  — 


0.0112  x;/^ 


or 


^z  = 


(i  X  (j) 


0.0112  x?/"^x  0.035 


(3.28) 


The  steady  slate  error  can  be  minimized  by  increasing  the  nonlinear  contiol  gain 
{}]')  and  decreasing  boundary  layer  ('/i)  as  shown  in  Figuie  18  on  page  44.  How- 
ever, it  can  ne\er  be  completely  eliminated.  The  depth  response  oi^  the  Figure  18 
(a)  sliow  the  steady  state  error  for  the  constant  disturbance  (d  =0.005)  when  us- 
ing the  nonlinear  feedback  gain  (ETA  =  4).   Large  nonlinear  feedback  gain  is  used 
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in  order  lo  decrease  the  steady  state  error  as  shown  in  Figure  18  (b).  The  steady 
state  error  for  the  command  depth  is  decreased  down  to  10%  approximately  us- 
ing large  gain.  However,  dive  plane  is  kept  the  same  angle  regardless  nonlinear 
feedback  gain.  If  we  use  large  gain  and  small  boundary  layer,  the  depth  response 
of  the  AUV  may  overshoot  and  the  di\c  plane  input  may  have  numerical  chat- 
tering. This  is  different  from  the  chattering  due  to  disturbance  and  unmodeled 
dynamics  as  shown  in  Figure  19  on  page  45.  This  numerical  chattering  problem 
is  an  artifact  of  the  numerical  integration  method  (a  fixed  step  Kutta-Merson  and 
fixed  Euler's  method  were  used  in  the  simulation)  and,  in  principle,  it  can  be  re- 
duced by  choosing  a  more  accurate  method  with  smaller  time  step. 
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The  steady  state  error  for  increasing  nonlinear  feedl)ack  gain 
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Figure   19.      The  numerical  chattering  problem 
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5.     Robustness  tests 

Another  important  aspect  of  the  shding  mode  control  is  its  robustness  to 
parameter  variations  and  unmodeled  dynamics.  Sliding  mode  controllers  are  ex- 
pected to  outperform  more  conventional  linear  state  feedback  with  respect  to 
robustness.  This  is  true  for  the  following  two  reasons:  First,  choosing  rj^  suffi- 
ciently large  will  guarantee  that  the  system  approaches  the  sliding  surface 
asymptotically  even  for  the  off-design  case.  Second,  since  at  the  final  stage  the 
system  evohes  in  a  lower  dimension  state  space  (the  sliding  surface),  it  is  na- 
turally more  robust  than  the  original  higher  dimensionality  system.  To  verify 
these  expectations,  we  changed  the  hydrodynamic  coefficient  of  the  AUV,  AUV 
rpm  and  di\"e  plane  strength  as  in  Table  4.  The  standard  design  AUV  equation 
are  designated  by  A,  B  matrices  at  6  ft/sec  (rpm  =  500). 

Table  4.       ROBUSTNESS  TEST  CASES 


TEST.  NO 

A  MATRIX 

B  MATRIX 

SPEED(RPM) 

Nominal  test 

A 

B 

6  ft  sec  (500) 

Test  1 

2\\ 

B 

6  ft  sec  (500) 

Test  2 

A 

2*B 

6ft  sec  (500) 

Test  3 

A  2 

B 

6  ft  sec  (500) 

Test  4 

A 

B  2 

6  ft  sec  (500) 

Test  5 

2 '''A 

2*B 

6  ft  sec  (500) 

Test  6 

A  2 

B  2 

6  ft  sec  (500) 

Test  7 

A 

B 

12  ft  sec  (1000) 

The  sliding  mode  control  for  the  AUV  was  designed  based  on  the  nominal  case 
as  in  the  pre\ious  section.  The  nominal  nonlinear  feedback  gain  (?/^  =  2.4)  and 
boundary  layer  thickness  (0  =  0.4)  were  applied  to  the  modified  system.  The  dy- 
namic response  of  the  modified  AUV  is  shown  in  Figures  14,  and  20,  to  26.  The 
A  matrix  of  test  1  model,  which  is  related  to  the  rotary  damping  coefficient,  was 
doubled  in  magnitude  in  order  to  test  the  sliding  mode  control.  Only  the  response 
time  of  test  1  model  was  longer.    The  test  3  model,  which  has  a  matrix  equal  to 
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one-half  of  the  A  matrix,  presents  fast  response  and  overshoot  due  to  decrease  in 
the  the  rotary  damping  moment  as  shown  in  Figure  22.  The  hydrodynamic  coef- 
ficient for  the  dive  plane,  which  in  proportional  to  dynamic  response  time,  was 
changed  in  order  to  test  performance  of  variable  structure  system  for  the  AUV. 
In  general,  it  is  evident  from  the  simulation  results,  that  even  under  a  200% 
change  in  the  coefficient  of  the  A,  B  and  RPM,  the  vehicle  response  remains 
stable,  7  ~  8  %  overshoot,  faster  or  slower  as  expected. 
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Figure  20.      The  response  of  2*A,  B  and  500  rpm 
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Figure  24.      The  response  of  2 -A,  2"B  and  500  rpni 
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Figure  25.      The  response  of  A/2,  B/2  and  500  rpni 
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D.     DESIGN  OF  SLIDING  MODE  CONTROL  FOR  THE 
NONLINEAR  MODEL 

The  sliding  mode  appeal  is  based  on  its  ability  to  treat  nonlinear,  time  varying 
and  unmodelled  systems  in  a  straightforward  manner  [Ref.  3].  It  is  the  purpose 
of  this  section  to  apply  the  linear  sliding  mode  controller  to  the  nonlinear  model 
of  the  actual  AUV.  The  sliding  surface  and  control  law  gain  for  the  actual  AUV 
are  based  on  the  linearized  nominal  model.  The  mathematical  form  of  the  non- 
linear model  for  the  actual  AUV  is 


dx 
dt 


^Ax+^f[x)+B{x)u+d{x,  t) 


(3.29) 


where        B{x)  is  the  nonlinear  function  associated  with  the  control 
surface  and  actuator  system 

A/(.t)   is  the  uncertainty  of  the  nonlinear  function  f(x). 
^^(.v.  t)   is  the  uncertainty  disturbance. 


The  system  matrix  A  is  the  estimate  of  f(x)  and  the  magnitude  of  the  uncertainty 
is  bounded  as 


T , 


F>\S'^J\x) 


(3.30) 


where      F  = 


F, 
F- 


F„ 


The  indi\idual  bounds  on  any  element  of  A/ as  estimated  from  some  knowledge 
of  the  extremes  of  possibility  o^  ^J\x).  Also,  let  B(x)  be  approximated  by  B,  a 
constant,  where  the  \arying  gain  ft  is  defined  by  B=  pB{x).  and  [i  is  taken  to  be 
scalars.  but  bounded  within  the  limits  of  }j^^  <  P  <  /^max»  and  li„om:r,ai  ="  1-   Then  it 
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follows  that  P  =  \,  and  A/=  0,  and  d(x,t)  =  0  will  yield  the  nominal  sliding  mode 
control  law  (3.20).  Disturbance  d(x.t)  is  unknown  but  is  upper  bounded  by  a 
known  continuous  function  such  that: 

Z)>|5^J(x,  01  (3.31) 

The  dynamics  of  the  system  with  bounded  uncertainty  of  the  actual  AUV  is 

dx 


dt 


=  Ax+Af{x)+PBu+d{x,  t)  (3.32) 


This  equations  is  used  to  simulate  the  uncertain  nonlinear  terms  after  the 
linearization  process.  We  have  designed  the  sliding  surface  g{x)  =  S'^x  by  using 
the  nominal  linearized  model  in  the  previous  section.  Using  the  sliding  condition 
theory  for  the  actual  AUV.  the  sliding  mode  control  law  for  the  nonlinear  model 
can  be  chosen  from  equations  (3.17)  and  (3.18)  such  that  if 

u{x)a{x)  =  -ij'  I  a{x)  \ 

and  the  system  will  reach  the  surface  iu{x)  =  0)  within  a  finite  time  t,  defined  by 

-     ..^^  •  (3.33) 


The  true  dynamics  of  the  sliding  surface  with  uncertainty  are,  howe\er,  given  by 

d{x)  =  5^A- 

=  s'^lAx+Atlx)+[]Bu+d{x,  ?)]  (3.34) 

Substituting  equation  (3.20)  to  the  abo\e  equation  for  u,  then  the  dcri\ati\"es  of 
(^, 

a  =  {\-[r^)S'^Ax+s'^Aflx)+s'^d{x,  t) 

-tr\fsign{o)  (3.35) 
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From  the  abo\e,  stability  is  guaranteed,  if  and  onh^  if 

(t{x)  <  -nlsign[a),  (3.36) 

where       ^/5  is  the  nonlinear  feedback  gain  without  an  uncertainty 

If  the  control  matrix  B  is  exactly  known  (/?  =  1),  then  control  law 

u^  =  -[s'^BY^s'^Ax-ls'^BT^rsigniaix))  (3.37) 

where        if  >  I  nl+F{x)+D{x,  t)\  , 

can  guarantee  stability  and  perfect  tracking  for  the  nonlinear  system  with  con- 
stant control  matrix  of  the  AUV.  In  case  where  the  control  system  B(x)  is  un- 
certain, the  following  change  (if)  must  be  made: 

u  =  -[S^  BY^  s'^Ax-[s'^BY\]^signa{x)  (3.38) 

u  =  u+u 

where        u  =  -[S^BYS^Ax 

u  =  -lS^Byhfsign{(7) 

if  >  L..  I  m+F{^:}+D{x,  t)\  +  \  (/?_-!)  i  I  SL4x  | 

Since  the  nonlinear  term  of  the  B(x)  and  Af\x)  is  uncertain  in  most  cases,  it  is 
assumed  to  be  zero  equation  (3.29)  and  if  is  increased  depending  on  their  as- 
sumed bonuds  to  guarantee  stable  sliding  mode  control  [Ref.  2].  The  actual 
control  law  used  in  subsequent  simulation  was  in  fact  equation  (3.38)  with  suffi- 
ciently large  ?/-  to  accomodate  the  uncertain!}"  in  Af\x)  and  B(x).  The  sliding 
mode  switching  control  law  equation  (3.38)  guarantees  that  equation  (3.18)  is 
satisfied  e\  en  in  the  presence  of  parameter  variations  and  unmodeled  dynamics 
pro\idcd  1]'  is  large  enough.  The  di\e  plane  angle  chattering  due  to  modeling  er- 
rors and  disturbances  can  be  eliminated  by  defining  a  boundary  layer  thickness 
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(f)  about  (7  =  0  as  illustrated  in  the  pre\ious  subsection.  The  smooth  sliding  mode 
control  law  of  the  actual  AUV  is  then 

u  =  -\_S^BT^(s'^Ax+}rsatsign{-j-)]  (3.39) 

where       r  >  Ama.  1  if6+f{^-)+D{x,  0  I  +  I  iP,,^- 1)11  S^Ax  \ 

The  dynamic  response,  dive  plane  angle,  and  sliding  surface  obtained  from  non- 
linear model  simulation  at  500  rpm.  are  shown  in  Figure  27  on  page  59.  Al- 
though the  sliding  surface  and  control  law  gain  based  on  the  nominal  linear 
system  were  applied  to  the  nonlinear  system,  the  response  of  the  system  is  satis- 
factory as  expected.  The  designed  variable  structure  system  based  on  the  nominal 
linear  equations  dealt  with  the  full  nonlinear  dynamics  of  the  AUV  as  shown  in 
Figure  27.  The  technique  of  sliding  mode  control  can  handle  the  nonlinear  sys- 
tem directly  without  linearization,  if  sliding  surface  coefficients  are  properly  cho- 
sen. This  is  especially  important  for  highly  maneuNcrablc  underwater  vehicles 
that  can  mo\"e  in  all  directions. 
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E.     ROBUSTNESS  TESTS  USING  THE  NONLINEAR  MODEL 

The  \ariable  structure  systems  ha\e  an  important  property  which  is  much  less 
sensiti\e  to  nonlinearities.  disturbances,  and  unmodeled  dynamics  as  illustrated 
in  the  pre\ious  chapter.  The  nonlinear  model  of  the  AUV  is  used  to  demonstrate 
the  properties  of  this  control  methodology.  Accurate  models  are  normally  re- 
quired in  order  to  achie\e  good  control.  Ho\ve\er,  since  hydrodynamic  modeling 
is  a  key  element  in  the  design  of  control  system  for  AUV,  we  have  to  rely  on  ap- 
proximate expressions  for  the  hydrodynamic  forces  which  can  introduce  large  er- 
rors into  the  control  process.  We  have  already  seen  the  results  of  simulation  for 
control  performance  using  an  accurate  linear  model,  modified  linear  model  and 
accurate  nonlinear  model.  In  this  section,  a  simulation  study  is  performed  to  il- 
lustrate the  effecti\eness  of  the  sliding  control  under  large  modifications  in  the 
nonlinear  model  parameters.  The  modified  hydrodynamic  coefficients  in  Table 
4  are  closely  related  to  s\stem  stability.  The  rotary  damping  coefficient  (A/^)  af- 
fects the  hydrodynamic  moment  of  the  AUV  in  the  vertical  plane.  The  center  of 
gra\ity  center  of  buoyancy  separation  is  directly  proportional  to  restoring  mo- 
ment. Hydrodynamic  coefficient  (Ms.)  directly  affects  the  pitch  moment  gener- 
ated by  the  di\e  planes.  Speed  was  changed  from  500  rpm  to  1000  rpm. 


Table  5.       TEST  CASES  FOR  THE  NONLINEAR  MODEL 

TEST  NO 

-^A. 

Z, 

■^/y. 

RPM 

Test  1 

Z. 

M,. 

500 

Test  2 

2^\/, 

Ze     2 

2^u,, 

500 

Test  3 

■^r, 

A 

2^^4 

500 

Test  4 

-'I/.  - 

Z.   2 

A4   2 

500 

Test  5 

.\/, 

Z, 

M,.   2 

500 

Test  6 

■^/; 

Z,   4 

M, 

500 

Test  7                     .\/, 

Z, 

-^/.:' 

1000 

The  controller  used  was  based  on  the  nominal  linear  model,  while  some  \ehicle 
coefficients  differed  bv  200" o  and  nonlinear  terms  were  added.  The  FORTRAN 
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program  in  Appendix  B  was  used  for  this  purpose.  Time  plots  of  depth,  pitch 
rate,  sliding  surface,  and  pitch  angle  response  along  with  the  depth  commanded 
values  are  shown  from  Figure  28  on  page  62  to  Figure  33  on  page  67. 
The  sliding  mode  controller  is  shown  to  provide  stable  response  and  performs 
consistently  at  different  parameter  values  and  speed.  In  every  case,  the  system 
remains  inside  the  boundary  layer  (a  —  0).  It  is  felt  that  the  sliding  mode  method 
can  produce  extremely  robust  controllers  that  perform  predictably  despite  the  use 
of  simplified  or  unmodeled  dynamics.  The  center  of  gravity  of  the  AUV  is  the 
most  sensitive  parameter  in  the  \ehicle  dynamic  response.  The  depth  response 
of  the  AUV  did  not  o\ershoot.  although  the  center  of  gravity  was  close  to  the 
center  o\^  buo\ancy.  In  general,  the  rpm  affected  the  dynamic  response  of  the 
nonlinear  vehicle.  In  this  case,  although  the  rpm  was  doubled  in  magnitude,  the 
o\ershoot  was  not  present  and  the  response  time  reduced  to  35  seconds.  When 
variation  of  parameters  and  modeling  error  of  the  AUV  were  increased,  the 
robustness  of  the  sliding  control  was  impro\ed  by  increasing  the  nonlinear  feed- 
back gain  (//-).  Of  course,  if  the  uncertain  \alues  exceed  the  limited  boundary, 
the  sliding  mode  control  will  not  handle  these  variations  effectively.  The  chosen 
nonlinear  feedback  gain  (?/-  =  2.4)  for  the  selected  AUV  was  enough  to  handle  the 
nonlinear  terms  and  the  modified  hydod\'namic  coefficients.  The  modeling  er- 
rors, variations  of  parameters,  and  nonlinear  terms,  which  were  difficult  problems 
of  robust  using  other  control  technique,  were  easily  dealt  with  using  the  sliding 
mode  control  as  shown  from  the  results  of  simulations. 
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IV.     DESIGN  AND  EVALUATION  OF  A  SLIDING  MODE 

COMPENSATOR 

A.  GENERAL 

An  autonomous  underwater  vehicle  (AUV)  must  operate  under  its  own  power 
and  be  capable  o[  navigation  and  guidance  with  sufficient  accuracy  to  be  easily 
recoverable.  A  navigation  system  for  an  underwater  vehicle  is  subject  to  vehicle 
size  or  cost  restrictions  and  this  limits  the  ability  to  install  the  highly  accurate 
sensors  needed  to  produce  reliable  pitch  rate  data.  The  vehicle  depth  can  be 
measured  directly  by  a  pressure  cell  sensor  aboard  the  vehicle.  Using  the  depth 
of  the  vehicle  as  the  only  external  input,  state  observers  can  provide  all  remaining 
controller  data.  It  is  desirable  to  investigate  the  performance  of  a  sliding  mode 
compensator  designed  for  a  linear  model  and  applicable  to  a  nonlinear  model 
with  unmodeled  \ehicle  dynamics.  It  is  the  purpose  of  this  chapter  to  design 
such  a  sliding  mode  compensator  and  assess  its  robustness.  This  work  invokes 
numerical  simulation  of  the  performance  of  a  sliding  mode  compensator  scheme 
using  a  linear  and  a  lull  12  state  nonlinear  model  of  the  equation  of  motion  for 
the  AUV. 

B.  DESIGN  OF  SLIDING  MODE  COMPENSATOR 

L     Linear  Model 

A  state  obser\er  is  designed  which  uses  measured  depth  only  in  order  to 
estimate  the  \  ehicle  pitch  rate  and  pitch  angle.  The  block  diagram  of  the  basic 
sliding  mode  compensator  is  illustrated  in  Figure  35  on  page  71.  The  state  ob- 
ser\er  design  is  based  on  the  equation  [Ref.  1 1]. 

A-  ^A.x+Bu+KQiv-Cx)  (4.1) 

where  x  is  the  state  variable  estimated  by  the  observer,  and  Kq 
is  the  obscr\"er  feedback  which  is  required  for  the  obser\er  to  be  able  to  follow 
and  duplicate  control  system  operation. 
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A,  B,  and  C  are  the  original  open-loop  control  matrices  of 
equation  (3.1 1). 

u  is  the  sliding  mode  control  law  based  on  the  observer. 

Note  that  in  the  sliding  mode  compensator,  disturbance,  nonlinear  terms,  and 
variation  of  parameters  are  ignored.  Equation  (4.1)  is  schematically  depicted  in 
Appendix  A.   Collecting  terms  and  rearranging 

X  =  {A-K^^C)x  +  Bu+K^  (4.2) 

which  is  then  rearranged  into  the  familiar  state  space  form: 

A'  =  Ax+Bu 

y  =  Cx+bu  (4.3) 

where        A  is  the  observer  A  matrix  {A—KqC) 
B  is  the  observer  B  matrix 
C  is  the  obser\'cr  C  matrix 

The  values  for  obser\'er  feedback  gain  (A'o)  are  calculated  using  the  Matrix-x 
program  "OBSERVER"  in  Appendix  A.  This  pro\ids  three  values  of  gain  for  the 
one  state  (depth)  that  is  used  by  the  observer  in  order  to  estimate  the  remaining 
two  (pitch  rate  and  pitch  angle).  The  poles  chosen  for  the  obser\er  are  -4.5.  -4.75, 
and  -4.95.  These  \alues  are  selected  because  they  create  a  faster  response  in  the 
obser\er  than  in  the  controller  itself.  This  condition  is  necessary  to  ensure  that 
the  obser\er  will  not  slow  down  the  o\erall  simulation  speed  of  the  controller. 
The  sliding  surface  of  the  sliding  mode  compensator  has  the  same  form  as  the 
sliding  surface  of  a  controller  a(x)  =  S^x. 

The  state  obser\er  pro\ides  necessary  information  to  the  sliding  mode  controller. 
The  control  input  of  the  sliding  mode  compensator  can  be  described  in  the  fol- 
lowing process.    From  the  sliding  condition  we  ha\e 
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Figure  35.      The  block  diagram  of  the  sliding  mode  compensator, 
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&{x)  =  -}]^sign{u)  (4.4) 

Substituting  equation  (4.3)  to  equation  (4.4)  and  rearranging 

S  X  =  —t]  sign{a) 

s'^{A.x  +  Bu)  =  -}f^sign{a)  (4.5) 

Finally,  the  sliding  mode  control  law  can  be  expressed  in  terms  of  the  estimated 
state  variables  in  the  following  form: 

u  =  -{s'^B)~^S^'Ax-{s'^Br^}rsign{c)  (4.6) 

or 

A        _ 

u  =  u+u 

where         u  =  -{S^Br^S'^Ax 

f7-  -{S^B)-hrsign{G) 

The  Figure  36  shows  the  dynamic  response  of  the  \chiclc  with  the  sliding  mode 
compensator.  The  nonlinear  feedback  gain  (?/-)  in  the  compensator  is  the  same 
as  the  gain  in  the  controller  without  the  observer.  It  can  be  seen  that  since  the 
obser\er  poles  are  fast,  the  performance  of  the  current  compensator  resemble  that 
of  the  controller  alone  of  Chapter  3.  A  sliding  mode  compensator  is  easier  to 
design  and  implement  than  con\cntional  compensators. 
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2.     Nonlinear  Model 

The  designed  nonlinear  control  feedback  gain  (if),  sliding  surface  (a{x)), 
and  obser\er  feedback  gain  (AV).  which  are  based  on  the  nominal  linear  model, 
are  used  to  design  a  sliding  mode  compensator  in  order  to  handle  a  full  nonlinear 
model.  Since  a  nonlinear  model  can  never  be  as  easily  predictable  as  a  linear 
model,  there  exists  certain  degree  oi'  uncertainty  in  the  nonlinear  terms.  A  similar 
process  as  in  Chapter  3  provides  a  design  of  a  nonlinear  sliding  mode 
compensator.   The  AUV  nonlinear  dynamics  is  given  by 

X  =  Ax+Af{x)+B{x)u  (4.7) 

where      Af{x) ,     B{x)  are  the  model  errors  of  nonlinear  terms  and 

uncertainty. 

Equation  (4.7)  can  be  rearranged  by  using  the  same  proceedure  in  Chapter  3. 

A-  =  Ax+AJ{x)+PBu  (4.8) 

The  procedure  for  obtaining  x{t)  of  x(t)  is  to  compute  the  estimate  to  be  the  out- 
put of  the  dynamic  system. 

X  =  Ax+AJ\x)+pBu+K^(\--Cx)  (4.9) 

We  can  rewrite  (4.9j  in  the  following  state  space  form 

x  =  Ax+AJ\x)+l]Bu 

y  =  Cxu  (4.10) 

where       Fix)  >  \  S^Af{x)  | 
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The  closed-loop  dynamics  of  the  state  observer  on  the  sliding  surface  with  un- 
certainty is  civen  bv 


g{x)  =  S  X 

=  S^lAx+AJ\x)+PBu^     '      .  (4.11) 

Substituting  equation  (4.6)  to  the  above  (4.1 1)  for  u,  then  we  obtain  the  following 
equation 

a  =  {\-r^}S'^Ax+s'^Af{x) 

-r'rrsignuix)  (4.12) 

If  (7  <  -}fQsignG{x)  is  enforced,  stability  will  result.  We  can  establish  conditions 
on  the  use  of  ?/-  in  equation  (4.6)  that  will  guarantee  that  sliding  condition,  given 
the  bounds  of  uncertainty.  It  follows  that 

r  >  Piul+s'^AJlxj+il-rWA^^)  (4.13) 

which  can  be  achie\ed  by 

'/'  ^  /^max  I  '?0  +  ^(^')  1  +  I  (/^max-1)  I  I  ^''Ax  \  (4.14) 

These  uncertainties  in  equation  (4.10)  are  eliminated  by  using  equation  (4.6)  but 
with  large  enough  gain  (?/^)  in  order  to  guarantee  stability.  The  boundary  layer 
used  in  numerical  simulation  was  selected  in  order  to  have  the  interpretation  of 
smoothing  out  the  discontinuity  in  the  nominal  control  law  at  the  switching  sur- 
face.  The  nominal  control  law  considered  in  this  section, 

u  =  -[s'^BT^S^Ax-ls'^BT^rj'satsignl  — ^  j  (4.15) 

where        if  >  L.<  1  m+F{x)  |  +  |  {(^^^-i)  \  |  S^Ax  \ 


75 


guarantees  asymptotic  stability  for  tlie  nonlinear  model  which  have  state  observer 
as  shown  in  Figure  37.  The  nonlinear  feedback  gain  (if)  was  chosen  according 
to  the  assumed  bounding  nonlinear  terms.  The  selected  gain  (^7-)  in  the  previous 
chapter  is  large  enough  to  handle  any  unknown  bounded  uncertainty.  The  Fig- 
ure 37  depicts  the  expected  robustness  of  the  dynamic  response  using  sliding 
mode  compensator.  This  method  produces  an  extremely  robust  sliding  mode 
compensator  that  performs  predictably  despite  the  use  of  the  sliding  surface,  gain 
{}f),  control  law.  and  observer  feedback  gain  (A'o)  which  are  based  on  the  nominal 
nlinear  model.  The  FORTRAN  program  "OBSERVER"  in  Appendix  B  has  been 
written  to  implement  the  sliding  mode  compensator.  The  results  of  Figure  37  on 
page  77  were  obtained  by  using  the  standard  hydrodynamic  coefficients  of  the 
nonlinear  model  at  500  rpm. 
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C.     ROBUSTNESS  TESTS 
1.     Linear  Model 

In  the  prc\ious  numerical  analysis  we  saw  that  sliding  mode  compensator  could 
deal  with  unknown  nonlinear  terms  by  using  nonlinear  feedback  control  law  u. 
A  modified  system  matrix  A,  and  control  matrix  B  is  used  to  estimate  robustness 
performance  of  the  sliding  mode  compensator  in  this  section.  The  effects  of 
modeling  error  in  a  linear  model  can  be  compensated  by  considering  the  size  of 
modeling  error  as  in  the  classical  state  space  methods  [Ref.  11].  The  variations 
in  control  matrix  B  can  be  handled  using  a  similar  method.  This  technique  is  easy 
to  analyze  for  a  single  input  system.  The  effects  of  modeling  error  can  be  consid- 
ered for  any  system  using  sliding  mode  compensator.  Consider  the  effect  of  a 
variation  in  the  system  matrix  A  and  control  matrix  B.  The  system  dynamics  are 
given  by 

X  =  iA-\-dA)x+{B+5B)u  (4.16) 

where      5A  is  the  unknown  bounded  changing  sj^stem  matrix 
SB  is  the  unknown  bounded  changing  control  matrix 

The  unknown  changed  system  matrix  (SA)  and  control  {()B)  matrix  can  be  ac- 
commodated by  using  a  large  nonlinear  (?/-)  and  boundary  layer  ((/>).  The  large 
gain  will  guarantee  stability  and  the  boundary  layer  obtained  in  the  pre\ious 
chapter  will  eliminate  the  chattering  which  is  intrinsically  linked  to  the  use  of  a 
switching  surface.  The  FORTRAN  program  "OBSERVER"  in  Appendx  A  is 
used  to  estimate  unmeasurable  state  and  perform  sliding  mode  compensator  to 
the  modified  system.  The  same  boundary  layer  (o^),  nonlinear  feedback  gain  (?/'), 
control  law  (4.6),  and  obser\"er  gain(/^o)  ^^  in  the  nominal  system  are  applied  to 
the  modified  system  to  estimate  the  performance  of  a  sliding  mode  compensator 
designed  on  a  nominal  linear  model.  The  system  matrix  A  and  control  matrix  B 
are  modified  as  in  the  followins  Table  6. 
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Table  6.       ROBUSTNESS      TESTS      CASE      OF      THE      SLIDING      MODE 
COMPENSATOR 


Test  No 

Sy'^lem  matrix  A 

Control  matrix  B 

Speed 

Test  1 

2''^A 

B 

6  ft,  sec 

Test  2 

A  2 

B 

6  ft  sec 

Test  3 

A 

2=^B 

6  ft  sec 

Test  4 

A 

B  2 

6  ft  sec 

Test  5 

2*  A 

2'''B 

6  ft  sec 

Test  6 

A  2 

B  2 

6  ft  sec 

The  dynamic  response  for  each  test  is  shown  in  Figure  38  on  page  80  through 
Figure  43  on  page  85.  Although  the  system  matrix  A  and  control  matrix  B  are 
modified  by  200^''o,  there  is  only  10  -  1 1%  overshoot  to  the  test  5.  The  expected 
robustness  of  the  d\namic  response  is  presented  by  using  sliding  mode 
compensator  based  on  nominal  linear  model.  Sliding  mode  compensator  per- 
formance is  \erificd  for  a  linear  \ehiclc  with  uncertain  A  and  B  matrix  without 
considering  additional  design. 

The  sliding  mode  compensator  de\eloped  in  this  section  was  stable  and  insensi- 
tive to  change  in  hydrodynamic  coefficients  of  the  AUV.  It  therefore  appears  that 
N ariable  structure  systems  will  pro\ide  the  most  robust  design  for  a  sliding  mode 
compensator  that  needs  to  maintain  an  accurate  prediction  of  \ehicle  response 
under  \arvinc  conditions. 
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Figure  41.      The  dynamic  response  of  test  4  AUV  (depth  connn.ind=  100  ft) 
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2.     Nonlinear  Model 

The  siding  mode  compensator  based  on  the  nominal  linear  model  is  here  applied 
to  the  highly  nonlinear  vehicle  with  modified  hydrodynamic  coefficients  in  order 
to  estimate  its  robustness. 

The  hydrodynamic  coefficients,  which  affect  in  the  depth  change  maneuvering, 
are  modified  as  Table  5  in  the  previous  chapter.  Although  an  unmodeled  dis- 
turbance could  be  made  up  for  using  model  based  compensator,  parameter  mis- 
match was  determined  to  be  sensitive  to  the  used  method  [Ref.  12].  This  would 
reduce  the  robustness  of  the  controller  when  confronted  with  the  varying,  un- 
controlled condition  found  in  the  ocean  enviroment.  In  this  section,  sliding  mode 
compensator  is  used  to  in\estigate  robustness  for  control  of  depth  change  ma- 
neuvering in  the  face  of  unmodeled  nonlinear  terms  and  parameter  uncertainty. 
The  nonlinear  system  dynamics  for  this  purpose  were  described  by 

X  =  Ax+dAx+Jlx)+lB+5B+g{x)^u  (4.17) 

Since  5 A,  SB.  J\x),  and  g{x)  are  unknown  values  in  the  present  system  equation 
described  abo\e.  they  are  assumed  to  be  zero  and  rj^^  is  increased  through  numer- 
ical simulation  in  order  to  guarantee  sliding  mode  control  [Ref.  2].  The  nominal 
control  law  (4.6)  is  used  to  simulate  the  response  of  all  values  (q,  u.  z,  and  s)  both 
actual  and  estimated.  The  FORTRAN  program  "NSMC"  was  written  in  order 
to  simulate  numerical  experiments  pro\'ided  in  Appendix  B.  Note  that  in  this 
section,  a  soft  saturation  function  might  be  used  to  handle  the  highly  nonlinear 
model  within  the  boundary  layer.   The  soft  saturation  function  is 

SAT=i  —p-  )  X  absaix)  (4.18) 

This  saturation  function  produces  soft  inputs  al  approximately  zero  a(x)  values. 
The  dynamic  response  of  the  highly  nonlinear  AUV  is  shown  in  Figure  44  on 
page  87  through  Figure  49  on  page  92. 
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V .     CONCLUSIOiNS  AND  RECOMMENDATIONS 

A.  SUMMARY 

This  thesis  presented  an  appHcation  of  variable  structure  systems  for  the  ro- 
bust control  of  linear  and  nonlinear  systems  in  the  presence  of  disturbances  and 
parameter  variations.  The  designed  sliding  mode  compensator  based  on  this 
methodology  was  successfully  employed  to  provide  a  means  for  an  unmanned 
underwater  \ehicle  to  control  its  depth  under  high  nonlinearities.  In  this  section, 
we  summarize  the  sliding  mode  compensator  design  of  the  specified  AUV: 

1.  The  fully  nonlinear  equations  of  motion  are  linearized  and  simplified  (as  il- 
lustrated in  section  3.C)  in  order  to  produce  linear  equations  in  the  state 
space  form. 

2.  The  sliding  surface,  which  provides  desired  dynamics  of  a  closed-loop  sys- 
tem, is  designed  by  the  standard  pole  placement  method  using  the  state 
space  form. 

3.  The  sliding  mode  control  law,  which  consists  of  linear  feedback  and  nonlin- 
ear switching  feedback,  can  be  obtained  by  satisfying  the  sliding  condition. 

4.  Chattering  due  to  piecewise  discontinuous  feedback  is  eliminated  by  replac- 
ing the  switched  control  law  at  the  sliding  surface  by  continuous  variations 
across  a  thin  boundary  layer  neighboring  the  switching  surface. 

5.  The  nonlinear  feedback  gain  (if-)  and  boundary  layer  (0)  are  optimized  by 
a  series  of  numerical  experiments. 

6.  The  state  observer  is  designed  using  classical  methods, 

7.  The  sliding  mode  compensator  is  designed  by  combining  the  sliding  mode 
control  and  stale  obser\er. 

B.  CONCLUSIONS 

The  objccti\"e  of  this  work  to  design  and  analyze  a  sliding  mode  di\e  plane 
compensator  for  an  autonomous  underwater  \ehicle  has  been  achie\ed.  Realistic 
limitations  due  to  pitch  and  pitch  rate  sensors  non-a\'ailability  were  taken  into 
account.  The  x'ehicle  that  was  considered,  the  SDV  is  a  typical  AUV  and  its 
shape  and  characteristics  greatly  resemble  the  NPS  vehicle. 

The  conclusions  of  this  work  arc  summarized  below: 
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1.  A  procedure  for  computing  the  sliding  plane  was  established.  The  procedure 
is  very  general  and  can  be  applied  to  a  large  class  of  linearized  single  input 
systems.  Once  the  sliding  plane  coefficients  ha\e  been  determined,  the 
switching  feedback  control  law  follows  easily. 

2.  Chattering  problems,  characteristic  of  variable  structure  system,  were  re- 
duced or  eliminated  by  introducing  a  "boundary  layer"  in  the  switching  logic, 
without  N'iolating  the  sliding  condition. 

3.  Robustness  of  the  design  was  demonstrated  by  a  wide  variation  of  parame- 
ters of  the  linear  model.  Similar  robustness  characteristics  were  established 
when  a  full  order  observer  was  incorporated  in  the  design  in  order  to  provide 
estimates  for  pitch  angle  and  pitch  rate  based  on  depth  measurements  only. 

4.  The  control  design  that  was  based  on  a  linear  model  was  tested  against  the 
full  nonlinear  equations  of  motion.  Numerical  simulations  demonstrated  the 
ability  of  the  design  to  handle  unmodelled  dynamics  and  variation  of  the 
hydrodynamic  coefficients  and  geometric  parameters  of  the  vehicle. 

5.  A  visual  simulation  using  an  IRIS  graphics  workstation  was  used  [Ref.  13] 
in  order  to  \iew  the  dynamic  behavior  of  the  AUV  in  real  time  under 
closed-loop  sliding  mode  control.  The  vehicle  response,  Figure  50  on  page 
95,  was  seen  to  follow  the  predictions  that  were  based  on  the  linear  model. 

6.  Finally,  an  experimental  verification  was  attempted  on  the  NPS  protype  ve- 
hicle with  coefficients  the  same  as  the  ones  used  in  [Ref.  12  ].  A  discrete- 
time  sliding  mode  controller  was  designed  based  on  25  Hz  sample  rate  with 
controller  poles  at  [0.9,0.91,  1],  observer  poles  at  [0.78,0.79,0.80],  control 

law 

u  =  -0.9q-\.7m-2l.297Cvrsign{(7) 

u  -  -0.3386^-1.68880+(z-z^^J  (5.1) 

where        ;/-'  =  0.2  and  (p  =  \.0 

The  experimental  results  are  shown  in  Figure  51  on  page  96,  where  the 
commanded  depth  was  5  \olts  (1  ft  corresponds  to  3.1  volts). 

The  \ariable  structure  system  was  pro\cn  to  be  an  attracti\e  control  system  de- 
sign method  for  autonomous  underwater  \ehicles.  The  designed  sliding  mode 
compensator  based  on  this  methodology  dealt  with  the  dynamic  problems  of  the 

underwater  \'ehicle  with  sufficient  accuracv. 
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Figure  50.      Graphic  simulation  display 


C,     RECOMMENDATIONS 

Some  suggestions  for  future  research  are  as  fbilows; 


1.  Design  and  analyze  a  nonlinear  sliding  mode  compensator, 

2.  Evaluate  the  robustness  characierislics  of  a  digital  (discrete  time)  sliding 
mode  autopilot. 

3.  Extend  the  developed  methodology  in  the  problem  of  path  following 
autopilots  for  the  multiple  input  case  under  constant  or  random  disturbance. 
Introduction  of  integral  control  action  should  help  in  this  case  to  assure 
precise  path  tracking. 
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APPENDIX  A.     SIMULATION  PROGRAM  AND  BLOCKDIAGRAM  FOR 

LINEAR  EQUATION 

A.     MATRIX-X(SMC)  OF  THE  LINEAR  EQUATION 

****   MATRIX  PROGRAM  FOR  SLIDING  CONTROLLER   *** 

UX=6;  //  FORWARD  SPEED 

CP1=0.25;  //  CLOSED  LOOP  POLE 

CP2=0.27;  //  CLOSED  LOOP  POLE 

Sl=l.;  //  SLIDING  SURFACE  COEFFICIENT 

S2=( CP1+CP2 ) ;  //  SLIDING  SURFACE  COEFFICIENT 

S3=(CP1*CP2 )/(-UX)  //  SLIDING  SURFACE  COEFFICIENT 

T=[0:0.1:150. ] ' ; 

I  ETA  =6.0; 

GA3=-0.035; 

GA4=-UX; 

GA5=-0. 7; 

GA6=-0. 03; 

GA7=S2; 

GA8=S3; 

GA9=IETA; 

GA10=1; 

GAl=(GA6-S3*UX)/(-GA3) ; 

GA2=(GA5+S2)/(-GA3) ; 

INPUT( : ,1 )=10  0*ONES(T) ; 

INPUT ( : ,2)=0. *ONES(T) ; 

Y=SIM(T, INPUT) ; 

PLOT(T, Y( :, 1 ), 'UPPER  LEFT  XLABEL/TIME ( SEC ) /  YLABEL/... 

DEPTH  RESPOSE/TITLE/    DEPTH  COMMAND=  100  , STANDARD  AUV/') 

PLOT(T, Y( : , 2 ) , 'UPPER  RIGHT  XLABEL/TIME ( SEC ) /  YLABEL/  ... 

PITCH  ANGLE/TITLE/     CLOSED  LOOP  POLE=0 . 0 , -0 . 3 5 , -0 . 4 0/ ' ) 

PLOT(T, Y( : , 4 ) , ' LOWER  LEFT  XLABEL/TIME ( SEC ) /  YLABEL/  ... 

DIVE  PLANE  ANGLE( radins )/TITLE/  SPEED=6/' ) 

PLOT(T, Y( : , 5 ) , ' LOWER  RIGHT  XLABEL/TIME ( SEC ) /YLABEL/  ... 

SLIDING  SURFACE/TITLE/  DIS=0.2,  B,  NLINEAR , I ETA=6 . 0 , PI=0 . 4/ ' ) 
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B.     FORTRAN(SMC)  OF  THE  LINEAR  EQUATION 

C  *  LINEAR  AUV  MODEL 

C  *  SLIDING  MODE  CONTROL  PROGRAM  ^- 

C  *  DEPTH  CONTROL  IN  DIVE  PLANE  ^• 

C  *  BY  USING  RUNGE-KUTTA  FIFTH  ORDER  ^' 

/^  .^^  y«  .J^  O-  »U  .J-  ..'^  J.  .'-  J^  ^^  mS^  y.  -^^  ^f  .J.  -f-  ^r<  ^.  -J-  .J^  .JL.  ^«  .f.  y.  .JL.  .JL.  .J^  ^.  ^.  ^.  ^«  ^^  ^*^  J^  ^*.  y .  .J«  JL  ..•. 

C 

REAL" 8  TIME,QDOT,  ZDOT,  TDOT,THETA,ZPOS ,Q,DS 
REAL^'--8  S,  DE,  UHAT,  UBAR ,  PHI ,  SAT,EITA,COMZ 
C     V"\-vr-vv-V"V->-y.-  INITIAL  CONDITIONS  ^-Vf-Wrv-vrv-^'-.'-yf 

C 

TIME=0.  ODO 

Q=0.  ODO 

THETA=0. ODO 

ZPOS=0. ODO 
C  vrv.-v.-,vv.-,vv.-y.-,v   SLIDING  MODE   CONTROL  PARAMETER  Vrv.-VfVryryfycVcVcV.-yf 

EITA=4. ODO 

DELT=0. 02D0 

COMZ=100.  ODO 

PHI=0.4D0 
C  VfVfVfVcVrVcVr   SYSTEM  PROGRAM  v-VfV-*v-v-vcVc 

WRITE   (8,710) 

DO  100  1=0,6000 
710   FORMAT  (3X,'    TIME',5X,'    DEPTH  ',6X,'       DIVE  ',5X,'     PITCH 

^'-',5X,'      SLIDE') 
C 

QDOT=-0.  7D0--'-Q-0.  OSDO-THETA-O.  035D0^'-DS 

TDOT=Q 

ZD0T=-12.  ODO^'-THETA 
C 

TIME=TIME+DELT 

Q=Q+DELT"QDOT 

THETA=THETA+DE  LT^'-'TDOT 

ZPOS=ZPOS+DELT-'--ZDOT 

S=Q-l-0.  52D0^'--THETA-0.  0112D0'''(  ZPOS-COMZ) 

IF  (ABS(S)  .LT.  PHI)  SAT=(S/PHI) 

IF  (S  .LT.  -PHI)  SAT=-1.  ODO 

IF  (S  .GT.  PHI)  SAT=1.  ODO 

UHAT=-5.  1429D0---Q+1.  07  lADO^'^THETA 

UBAR=EITA-'"^SAT 

DE=UHAT-I-UBAR 

IF  (DE  .GE.  0.4D0)  DS=0.  4D0 

IF  fDE  .LE.  -0.4D0)  DS=-0.  4D0 

IF  ((DE  .  LT.  0.4D0)  .AND.  (DE  .  GT.  -0.4D0))  DS=DE 

WRITE  (8,720)  TIME,  ZPOS ,  DS ,  THETA ,  S 
720   FORMAT  (  2X,E11.  3  ,4X,E11.  3  ,4X,E11.  3  ,  4X,E11.  3  ,4X,E11.  3) 
100    CONTINUE 

STOP 

END 
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1]- 

// 

DESIRE 

POLE 

OF 

OBSERVER 

// 

DESIRE 

POLE 

OF 

OBSERVER 

// 

DESIRE 

POLE 

OF 

OBSERVER 

C.     ]\IATRIX-X(OBSERVER)  OF  THE  LINEAR  EQUATION 

***  SIMULATION  PROGRAM  FOR  SLIDING  MODE  COMPENSATOR  *** 


UX=6;  //  FORWARD  SPEED 

CP1=0.3  //  CLOSED  LOOP  POLE 

CP2=0.40;  //  CLOSED  LOOP  POLE 

Sl=l.;  //  SLIDING  SURFACE  COEFFICIENT 

S2=(CP1+CP2 ) ;  //  SLIDING  SURFACE  COEFFICIENT 

S3=(CP1*CP2 )/(-0.6 ) ;  //  SLIDING  SURFACE  COEFFICIENT 

EITA  =6.0; 

A=[-0.7,-0.0  3,0;l,0,0;0,-6,0] ; 

B=[-0.0  35;0;0] ; 

C=10,0,1]; 

N=[C' ,A' *C' ,A' *A' *C' 1 ; 

W=  [1,0. 7, 0.03; 0,1, 0.7; 0,0 

LA1=5. 5; 

LA2=5.75; 

LA3=5. 9; 

ABAR=[ 0 .7 ;0.03 ;0 ] ; 

AHAT=[ LA1+LA2+LA3 ; LAl * LA2+LA2 * LA3+ LAl * LA3 ; LAl * LA2 * LA3 ] 

KC=INV( (N*W) ' ) *(AHAT-ABAR) ; 

GAl=-l . 4 ; 

GA2=-0 .06 ; 

GA3=-2*UX; 

GA4=KC( 3,1 ) 

GA5=KC( 2,1 ) 

GA6=KC(1,1 ) 

GA7=-0.7; 

GA8=-0.03; 

GA9=-6; 

GAlO=-0.035; 

GAl6=-0.035; 

GA11=0.75; 

GAl3=-0.0233; 

GAl4=(GA2-S3*UX)/(-GAlO) ; 

GAl2=(GA7+S2)/(-GAlO) ; 

GA15=EITA; 

T=[0:0.1:150.  ]  '  ; 

INPUT( : , 1 )=10  0*ONES(T) ; 

INPUT( : ,2 )=0.0*ONES(T) ; 

Y=SIM( T, INPUT) ; 

PLOT(T, ( Y( : , 1 )  Y ( : , 5 ) ] , ' LINE  STYLE  1  2  /UPPER  LEFT  XLABEL/ 

TIME(sec)  /  YLABEL/EST  AND  ACT  DEPTH(ft)/') 

PLOT(T, [ Y( : ,2 )  Y ( : , 4 ) ] , ' LINE  STYLE  1  2  /UPPER  RIGHT  XLABEL/ 

TIME(sec)/  YLABEL/EST  AND  ACT  ANGLE ( r ad ) /TI TLE/   B,  2*A/') 

PLOT(T, Y( : , 3 ) , ' LOWER  LEFT  XLABEL/TIME ( SEC ) /  YLABEL/ 

DIVE  PLANE  ANGLE/TITLE/  S.F.C=1,  0.75,  -0.0233,  SPEED-6/') 

PLOT( T, Y( : , 6 ) , ' LOWER  RIGHT  XLABEL/TIME ( SEC ) /YLABEL/ 

SLIDING  SURFACE/  TITLE/   SAT,  DIS=0.2,  I ETA=6 , PI =0 . 4/ ' ) 
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D.     FORTRAN(OBSERVER)  OF  THE  LINEAR  EQUATION 

C  *  LINEAR  AUV  MODEL 

C  *  SLIDING  MODE  CONTROL  PROGRAM 

C  *  DEPTH  CONTROL  IN  DIVE  PLANE     * 

C  *  BY  USING  EULER -METHOD         * 

C 

REAL''-8  TIME,QDOT,  ZDOT,  TDOT,THETA  ,  ZPOS  ,Q,DS 

REAL'VS  S,  DE,  UHAT,  UBAR,PHI  ,SAT,EITA,COMZ 

RE AL---8  THADOT ,  QHADOT ,  ZHADOT ,  QHAT ,  THAT ,  ZHAT 
C     ,v,Wf^v****,v  INITIAL  CONDITIONS  *'V,v,v,v,'ov,v,v,v 
C 

TIME=0. ODO 

Q=0.  ODO 

THETA=0.  ODO 

ZPOS=0. ODO 

QHADOT=0. ODO 

THADOT=0. ODO 

ZHADOT=0.  ODO 

QHAT=0.  ODO 

THAT=0. ODO 

ZHAT=0. ODO 
C     -.v,v-;.-,v,v-;oV-,v,v  SLIDING  MODE  CONTROL  PARAMETER  Vr,v,v,v,v,v,v,v,v,v,v 

EITA=4.  ODO 

DELT=0. 02D0 

COMZ=100. ODO 

PHI=0. 4D0 
C     -.vvr,vvr-;.-,v-;r  SYSTEM  PROGRAM  'V,v,v,v-;r-v,v,v 

WRITE  (8,710) 

DO  100  1=0,6000 
710   FORMAT  (3X,'    TIME',5X,'    DEPTH  ',6X,'       DIVE  ' ,5X, '     PITCH 

'■■-',5X,'      SLIDE') 
C 


QDOT=-0.  35D0''^Q-0.  015D0*THETA-0.  0175D0*DS 
TDOT=0.  5D0-'>Q 
ZD0T=-6.  0D0-->THETA 


TIME=TIME+DELT 
Q=Q+DELT^'-QDOT 
THETA=THETA+DELT^'^TDOT 
ZPOS=ZPOS+DELT^'--ZDOT 

,V-.VV.-V,-Vr-;.";.-Vr  SUBROUTINE  OBSERVER  V.-V.'yr,VVrVrV.-:?-V: 

CALL  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT) 
,v-;.";.",v-.vv.-,v  SLIDING  MODE  INPUT  -'"V,v,v,v,v,v-;c,v 

S=QHAT+0.  52D0^'-THAT-0.  01 12D0'-(  ZHAT-COMZ) 
IF  (ABS(S)  .LT.  PHI)  SAT=(S/PHI) 
IF  (S  .  LT.  -PHI)  SAT=-1.0D0 
IF  (S  .  GT.  PHI)  SAT=1.  ODO 
UHAT=-5.  1429D0^>QHAT+1.  0714D0^-THAT 
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UBAR=EITA'>SAT 

DE=UHAT+UBAR 

IF  (DE  .GE.  0.4D0)  DS=0.  4D0 

IF  (DE  .LE.  -0.4D0)  DS=-0.  ADO 

IF  ((DE  .  LT.  0.4D0)  .AND.  (DE  .  GT.  -0.4D0))  DS=DE 

WRITE  (8,720)  TIME,  ZPOS,  DS ,  THETA,  S 
720    FORMAT  (  2X,E11.  3  ,4X,E11.  3,4X,E11.  3  ,4X,E11.  3  ,4X,E11.  3) 
100    CONTINUE 

STOP 

END 
C     Vr,v-;cVf,v,v.v,v,v  SUBROUTINE  OBSER  vnv,v,v,v,v,v,v*,v 

C 

SUBROUTINE  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT) 
QHADOT=-0.  7D0-'>QHAT-0.  03D0-'^THAT-20.  9293D0'H  ZPOS-ZHAT) 
THAD0T=QHAT-14.  4092D0'nZPOS-ZHAT) 
ZHAD0T=-6.  ODO'VTHAT+16.  45D0'>(  ZPOS-ZHAT) 

QHAT=QHAT+DELT''^QHADOT 

THAT=THAT+DELT-->THADOT 

ZHAT=ZHAT+DELT-'-ZHADOT 

RETURN 

END 
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E.  FORTRAN  GRAPH 

DIMENSION  TIME(6000) ,DEPTH(6000) ,DIVE( 6000) ,PITCH( 6000) , 
•'■-SLIDE(6000) 
REAL  TIME , DEPTH , DIVE , PITCH , SLIDE 
DO  1  1=1,6000 
1  READ  (8,^--)  TIME(I),DEPTH(I),DIVE(I),PITCH(I),SLIDE(I) 
CALL  COMPRS 

C  v.-y:v.-yrv.-  CREATES  A  DUMMY  PLOT  FOR  POST  PROCESSING****** 
CALL  NOBRDR 
CALL  AREA2D(1.  ,1.  ) 
CALL  GRAF(0.  ,1.  ,20.  ,0.  ,1.  ,10) 
CALL  ENDPL(O) 

C  ic-hMcic   PLOT  COMMAND  DEPTH  ***** 
CALL  PAGE(3.  ,4.  ) 
CALL  NOBRDR 
CALL  AREA2D(2.  3,3.  0) 
CALL  XNAME( 'TIME(SEC)$' ,100) 
CALL  YNAME( 'COMMAND  DEPTH(FT) $ ' , 100) 
CALL  THKFRM(0.  03) 

CALL  GRAF(0.  ,30.  0,120.  ,-5. 0,25. 0,120. 0) 
CALL  THKCRV(0.  02) 
CALL  CURVE(TIME, DEPTH, 6000,0) 
CALL  ENDPL(O) 


'-**  PLOT  DIVE  PLANE  ANGLE  ***** 
CALL  PAGE(3.  ,4.  ) 
CALL  NOBRDR 
CALL  AREA2D(2.  3,3.  0) 
CALL  XNAME( 'TIME(SEC)$' ,100) 
CALL  YNAMECDIVE  PLANE  ANGLE(RAD)$  '  ,  100) 
CALL  THKFRM(0. 03) 

CALL  GRAF(0.  ,30.  0,120.  ,-.4,.  2,.  4) 
CALL  THKCRV(0.  02) 
CALL  CURVE( TIME, DIVE, 6000,0) 
CALL  ENDPL(O) 


r-.v,v  PLOT  PITCH  ANGLE  *^'-"'-"'--v.".v 
CALL  PAGET  3.  ,4.  ) 
CALL  NOBRDR 
CALL  AREA2D(2. 3,3. 0) 
CALL  XNAMEC 'TIME(SEC)$' ,100) 
CALL  YNAMECPITCH  ANGLE(RAD/SEC)  $  '  ,  100) 
CALL  THKFRMCO.  03) 

CALL  GRAF(0.  ,30.  0,120.  ,  -  1.  0  ,0.  3  ,  0.  2) 
CALL  THKCRVCO.  02) 
CALL  CURVECTIME, PITCH, 6000,0) 
CALL  ENDPL(O) 


•■**  PLOT  SLIDING  SURFACE  ***** 
CALL  PAGE(3.  ,4.  ) 
CALL  NOBRDR 
CALL  AREA2D(2.  3,3.  0) 
CALL  XNAME( 'TIME(SEC)$' ,100) 
CALL  YNA.MEC 'SLIDING  SURFACEC  RAD/SEC  )<;',  100  ) 


CALL  THKFRM(0.03) 

CALL  GRAF(0.  ,30.0,120.  ,-0.5,0.5,1.5) 

CALL  THKCRV(0.  02) 

CALL  CURVE( TIME, SLIDE, 6000,0) 

CALL  ENDPL(O) 

CALL  DONEPL 

STOP 

END  , 
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F.     BLOCK  DIAGRAM  OF  THE  MATRIX-X  SIMULATION 
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Figuie  52.      The  block  diagram  of  the  SMC  simulation 
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Figure  53.      The  block  diagram  of  tlie  SMO  siimilatioii 
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APPENDIX  B.     SIMULATION  PROGRAM  FOR  NONLINEAR  AUV 

NONLINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED  ^^ 

VARIABLE  DECLARATION  11  ^'-Z'   INPUTS  **  248 

^■-   CONSTANTS  70           PROPULSION  MODEL  271   -^^ 

•^'        INITIAL  CONDITIONS  128  **   OUTPUTS  -'-^-  423   * 

^•-   MASS  MATRIX  206           INTEGRATION  ^^ 

INVERT  MATRIX  245  ^- 

REAL  AW(82,82) 

REAL  MASS,LATYAW,NORPIT 

REAL  MM(6,6) ,G4(4) ,GK4(4) ,BR(4) ,HH(4) 

REAL  B(6,6),BB(6,6) 

REAL  A(12,12),  AA(12,12) 

REAL  XPP  ,XQQ  ,XRR  ,XPR 

REAL  XUDOT  ,X'VQ  ,XVP  ,XVR 

REAL  XQDS  ,XQDB  ,XRDR   ,XVV 

REAL  XlslV      ,XVDR    .Xlv'DS    ,XVDB 

REAL  XDSDS,XDBDB  ,XDRDR  ,XQDSN 

REAL  XIVDSN  ,XDSDSN 

REAL  TIME , S , E ITA , UBAR , UHAT , COMZ , BAR , S IM , DE , SAT 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  YPDOT  ,YRDOT,YPQ   ,YQR 
REAL  YVDOT  ,YP    ,YR    ,YVQ 
REAL  WP   ,WR  ,YV  ,YV\v' 
REAL  YDR   ,CDY 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  ZQDOT  ,ZPP,ZPR  ,ZRR 
REAL  ZWDOT  ,ZQ   ,ZVP    , ZVR 
REAL  ZW   ,ZVV    ,ZDS   ,ZDB 
REAL  ZQN  ,ZWN   ,ZDSN  ,CDZ 
REAL  ZHADOT,ZHAT 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

REAL  KPDOT  ,KRDOT  ,KPQ  ,KQR 

REAL  KVDOT  ,  KP   ,KR  ,KVQ 

REAL  KYP   ,  K\\'R    ,KV  ,KVW 

REAL  KPN    ,  KDB 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

REAL  MQDOT  ,MPP  ,MPR,MRR 

REAL  M\s"DOT  ,  MQ  ,MVP     ,MVR 

REAL  ^^^^'   ,  MVV  ,MDS   ,MDB 

REAL  MQX  ,  M\w\  ,MDSN 
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REAL  QHADOT,QHAT,THADOT,THAT 

YAW  HYDRODYNAMIC  COEFFICIENTS 

REAL  NPDOT,NRDOT,NPQ  ,NQR 
REAL  NVDOT   ,  NP    ,NR     ,NVQ 
REAL  NVP   ,  KVR   ,NV    ,NVW 
REAL  NDR 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

REAL  WEIGHT  ,  BOY   ,VOL  ,XG 

REAL  YG   ,  ZG   ,XB    ,ZB 

REAL  IX   ,  lY   ,IZ     ,IXZ 

REAL  lYZ   ,  IXT    ,YB 

REAL  L   ,  RHO  ,G     ,NU 

REAL  AO    ,KPROP  ,NPROP  ,     XITEST 

REAL  DEGRUD   ,DEGSTN 

COMMON  /BLOCKl/  F(12),  FP(6),  XMMINV(6,6),  UCF(4) 

INTEGER  N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,I 

REAL   WKAREA(54),  X(12) 

RUDDER  COEFFICIENTS 

PARAMETER  (  DSMAX=  -0.175) 

LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(XPP  =   7.E-3      ,XQQ  =   -1.5E-2    ,XRR  =  4.E-3      ,XPR  =7.5E-4, 
&  XUD0T=-7.  6E-3       ,X^Q  =   -2.E-1  ,XVP  =    -3.  E-3         ,XVR   =   2.E-2, 

&XQDS=2.5E-2  ,XQDB=-2.  6E-3  ,XRDR=-l.E-3         ,XVV=5.3E-2, 

&  XlslV  =1.  7E-1  ,XVDR=1.  7E-3  ,X'\\'DS=4.  6E-2  ,X'WDB=    1.  E-2 , 

&  XDSDS=    -l.E-2      ,XDBDB=    -8. E-3         ,XDRDR=    -l.E-2      ,XQDSN=   2. E-3, 
6c  X'WDSN=3.  5E-3         ,XDSDSN=   -1.6E-3    ) 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(YPD0T=1. 2E-4  ,YRD0T=1. 2E-3   ,YPQ=4.E-3  , YQR  =-6. 5E-3 , 
&  YVD0T=-5.5E-2   ,YP  =  3.  E-3      ,YR  =  3.  E-2      ,YVQ  =2.4E-2, 
6c  YWP  =2.3E-1     ,WR  =-1.9E-2     ,YV  =  -l.E-1     ,YVW  =6.8E-2, 
&  YDR  =2,  7E-2     ,CDY=3.5E-1) 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(ZQD0T=-6. 8E-3  ,ZPP=1.3E-4   ,ZPR=6. 7E-3   ,ZRR  =-7. 4E-3 , 
&  ZWD0T=-2.4E-1  ,ZQ  =-1.4E-l     ,ZVP  =-4.  8E-2    ,ZVR  =4.5E-2, 
6c  ZW  =   -3.E-1    ,ZVV=-6.8E-2     ,ZDS=-7.3E-2    ,ZDB  =-2.  6E-2 , 
&  ZQN  =-2.9E-3    ,ZWN  =-5.  lE-3     ,ZDSN=  -l.E-2    ,CDZ  =  1.0) 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(KPDOT=    -l.E-3    ,KRD0T=-3. 4E-5    ,KPQ  =-6. 9E-5    ,KQR=1.7E-2, 
&   KVD0T=1. 3E-4         ,KP=-l.lE-2  ,KR=-8.4E-4  ,KVQ=-5. lE-3 , 

&  K\vP   =-1.3E-4         ,    K^'R  =1.4E-2  ,KV  =3.  lE-3  ,KV^'  =-1.9E-l, 

&  KPN  =-5.  7E-4      ,    KDB   =   0.  0    ) 
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PITCH  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(MQD0T=-1. 7E-2    ,MPP  =5.3E-5,MPR  =  5.E-3,MRR  =-2.9E-3, 
&  MWD0T=-6.  8E-3   ,  MQ  =-6.  8E-2     ,MVP  =1.  2E-3    ,MVR=1.7E-2, 
6c  ^nv'  =  l.E-1      ,  MVV  =-2.6E-2    ,MDS  =-4.  lE-2    ,MDB  =6.  9E-3, 
&  MQN  =-1.6E-3    ,  MWN  =-2.  9E-3    ,MDSN  =-5.  2E-3) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER(NPD0T=-3.4E-5  ,NRD0T=-3. 4E-3 ,NPQ  =-2. lE-2  ,NQR=2.7E-3, 
&  NVD0T=1.2E-3    ,  NP  =-8.  4E-4     ,NR  =-1.6E-2     ,NVQ  =  -l.E-2, 
&  N\sT  =-1.  7E-2    ,  NVR=7.4E-3     ,NV  =-7.  4E-3     ,NVW  =-2.  7E-2 , 
&  NDR  =-1. 3E-2) 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

PARAMETER(  WEIGHT  =12000. ,  BOY  =12000.  ,VOL  =200.  ,XG  =  0. 

&  YG  =  0.  0  ,  ZG  =  0.2  ,XB  =  0.  ,ZB  =   0.0, 

&  IX  =  1500.  ,  lY  =  10000.  ,IZ  =  10000.  , IXZ  =  -10.  , 

&  lYZ  =  -10.  ,  IXY  =  -10.  ,YB  =   0.0  , 

&  L  =  17.4  ,  RHO  =  1.94  ,G  =  32.2  ,NU  =  8.  47E-4  , 

&  AO  =  2.0  ,KPROP  =  0.  ,NPROP  =  0.  ,  X1TEST=  0.  1  , 

&DEGRUD=0.0  ,DEGSTN=   0.0) 

INPUT  INITIAL  CONDITIONS  HERE  IF  REQUIRED 

UO  =  12.  0 
VO  =  0.0 
WO  =  0.  0 
PO  =  0.  0 
QO  =  0.  0 
RO  =  0.  0 
PHIO  =  0.  0 
THETAO  =  0. 0 
PSIO  =  0.  0 
XP0S0=0. 0 
YP0S0=0. 0 
ZP0S0=0.  0 
DB=  0.  0 
DS  =  0.  0 
DR  =  0.  0 
RPM  =  1000. 0 
LATYAW  =  0.0 
NORPIT  =  0.0 
RE  =  UO-L/NU 

U  =  UO 

V  =  VO 

W  =  WO 

P  =  PO 

Q  =  QO 

R  =  RO 

XPOS  =  XPOSO 

YPOS  =  YPOSO 

ZPOS  =  ZPOSO 

PS  I  =  PHIO 
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THETA  =  THETAO 
PHI  =  PHIO 
QHADOT=0. 0 
THADOT=0. 0 
ZHADOT=0.  0 
QHAT=0. 0 
THAT=0. 0 
ZHAT=0. 0 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

G4(l)  =  0. 069431844 
G4(2)  =  0. 330009478 
G4(3)  =  0. 669990521 
G4(4)  =  0. 930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

GK4(1)  =  0. 1739274225687 
GK4(2)  =  0. 3260725774312 
GK4(3)  =  0. 3260725774312 
GK4(4)  =  0. 1739274225687 

DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 

BR(1)  =  75. 7/12 
BR(2)  =  75. 7/12 
BR(3)  =  75.  7/12 
BR(4)  =  55.08/12 

HH(1)  =  16.38/12 
HH(2)  =  31.85/12 
HH(3)  =  31.85/12 
HH(4)  =  23.  76/12 

MASS  =  WEIGHT/G 


N  =  6 

DO  15  J  =  1,N 
DO  10  K  =  1,N 
XMM1NV(J,K)  =  0. 0 
MM(J,K)  =  0.  0 
10       CONTINUE 
15    CONTINUE 


MM(1,1)  =  MASS  -((RH0/2)^'-(L^--^'O)^--XUD0T) 
MM(1,5)  =  MASS^'-ZG 
MM(1,6)  =  -MASS-YG 
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,4) 

MM  (6 

,5) 

MM  (6 

.6) 

MASS  -((RHO/2)''^(L*''^3)*YVDOT) 
-MASS''^ZG  -((RH0/2)*(L**4)'VYPD0T) 
MASS-'^XG  -  ((RH0/2)''^(L''"'^4)''^YRD0T) 

MASS  -  ((RH0/2)'>(L''^'''3)''^ZWD0T) 

MASS'^YG 

-MASS*XG  -((RH0/2)*(L'"^*4)*ZQD0T) 

-MASS^'^ZG  -  ((RH0/2)'>(L*M)''^KVD0T) 

MASS*YG 

IX  -  ((RH0/2)'nL**5)*KPD0T) 

-IXY 

-IXZ  -((RH0/2)*(L**5)*KRD0T) 

MASS^ZG 

-MASS^XG  -( (RH0/2)*(L**4)'VMWD0T) 

-IXY 

lY  -((RH0/2)'KL*''^5)''^MQD0T) 

-lYZ 

-MASS'^YG 

MAS  S^>XG  -  ( ( RHO/  2  )  'H  L''"'^4  ) *NVDOT ) 

-IXZ  -  ((RH0/2)''-(L-"''5)--"^NPD0T) 

-lYZ 

IZ  -  ((RH0/2)'KL"*5)*NRD0T) 


20 


LAST  =  N-'-N+3--'^N 
DO  20  M  =  1,LAST 
WKAREA(M)  =  0. 0 
CONTINUE 


lER  =  0 

lA  =  6 

IDGT  =  4 

CALL  LINV2F(MM,N,IA,XMMINV,IDGT,WKAREA,IER) 


k*.  ^.  .t.  .1.  «*.  k*.  ^^  J^  ^^  .J^  ^«  ^. 


yrVfVf^fV.-Vr  INPUTS 

RUDDER  AND  DIVE  PLANE  COMMANDS 


DELT=0.  1 

SIM=  800. 0 

TIME=  0.  0 

DS=  0.0 

DR=  0.  0 

DB=  0.  0 

RPM=1000. 0 

EITA=6. 0 

COMZ=100.0 

BAR=0.4 

c 

SIZE  OF  OUTPUT  DATA  ARRAY  FOR  PLOTTING 

c 

NUM0UT=6 

c 

NUMPNT=4 

c 

WRITE  (8,711)  NUMPNT,NUM0UT 

no 


711 

C 
C 

c 
c 
c 
c 


WRITE  (8,710) 
FORMAT  (214) 
DO  100  1=1, SIM 


PROPULSION  MODEL 


SIGNU  =1.0 

IF  (U.  LT.  0.0)  SIGNU  =  -1.0 

IF  (ABS(U).LT.  XITEST)  U  =  XITEST 

SIGNN  =1.0 

IF  (RPM.  LT.  0.0)  SIGNN  =  -1.0 

ETA  =  0.  012''^RPM/U 

RE  =  U-'^L/NU 

CDO  =   .00385  +  (1.  296E-17)'"^(RE  -  1.2E7)'V'V2 

CT  =  0.  008--'^L'V-.V2'VETA-'^ABS(ETA)/(A0) 

CTl  =  0.  008-->L->--"-2/(A0) 

EPS  =  -1.  0+SIGNN/SIGNU--nSQRT(CT+l.  0)-l.  0)/( SQRT(CT1+1.  0)-l.  0) 

XPROP  =  CDO-'KETA-'''ABS(ETA)  -  1.0) 


CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 

INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 


500 
C 

c 
c 
c 
c 
c 


LATYAW  =0.0 
NORPIT  =  0.0 
DO  500  K  =  1,4 

UCF(K)  =  SQRT((V+G4(K)''fR*L)''"'^2 

IF(UCF(K).GT.  lE-10)  THEN 


+  (W-G4(K)''^Q-''L)''"V2) 


TERMO   =  (RH0/2)^'^(CDY''^HH(K)''>-(V+G4(K)''^R''^L)**2 
CDZ'--BR(K)-'>(W-G4(K)'VQ''^L)''^'>2) 
TERMO--'-(  V+G4 ( K )  •'•-R-'^L)  /UCF(  K ) 
TERM0-"-(W-G4(K)--'^Q^>L)/UCF(K) 
LATYAW  +  TERM1*GK4(K)'>L 
NORPIT  +  TERM2''-GK4(K)'>-L 


+ 


TERMl 

ter:i2 

LATYAW 

NORPIT 

END  IF 

CONTINUE 


FORCE  EQUATIONS 


LONGITUDINAL  FORCE 

FP(1)  =  MASS--'-V-"-R  -  MASS^'-W"Q  +  MASS^--XG-'--Q''"'^2  +  MASS^--XG^'R''-^-2- 
MASS--^YG^'-P--^Q  -  MASS-'-ZG^'-p-"-R  +  (RH0/2)''-L''"'^4'HXPP'-P"--'^2  + 
XQQ-->Q--'----2  +  XRR--'-R---"2  +  XPR^'-p-"-R)  +( RH0/2)^>L^--^'-3^'-(X"WQ^'^W^'^Q 
XVP--W-P+XVR"  V--'^R+U'>-Q-'K  XQDS'--DS+XQDB--^DB ) +XRDR"U^>R-->-DR  )  + 
(RH0/2)--'-L"-"-2^>-(XVV^W''^'--2  +  X'W^v-"W-"'-2  +  XVDR^-U^-V^--DR  +  U*W^- 
( Xl^'DS'-'^DS+XYDB-DB )  +U-''^^'-2-->(  XDSDS-''-DS-'"'^2+XDBDB-"-DB-'"->2+ 
XDRDR '--DR" "2 ))  -(WEIGHT  -BOY)--'-SIN(THETA)  +(RH0/2)^'-L''-^'^3''-- 
XQDSN-"-U-"-Q^"-DS--'-EPS+(RH0/2)-'-L-"'^2-'-(X¥DSN''-U''-W-->DS+XDSDSN'^U''"'-2-''^ 
DS-"---'--2  )  '--EPS  +(  RHO/  2  ) -',-l-'.".V2-.'.-u^'"'^2-"XPR0P 


+ 
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LATERAL  FORCE 

FP(2)  =  -MASS'^^U^'^R  -  MASS'>XG'>P''fQ  +  MASS*YG''fR''"'^2  -  MASS''^ZG^^Q'''R  + 
&       (RH0/2)''^L-'">4'HYPQ'^P''^Q  +  YQR-"-Q-'^R)+(RH0/2)''^L''^"3-"-(  YP-->U-''-P  + 
&       YR-'^U-'^R  +  YVQ'W"Q  +  YWP'^W-^P  +  YVR^W^'^R)  +  (RHO/2)->L''">2''^ 
&       ( YV'-U'W  +  YVW--W'-W  +YDR''^U*''f2'^DR)  -LATYAW  +( WEIGHT-BOY)* 
&       C0S(THETA)^>SIN(PHI)+MASS'>W'''P+MASS'>YG'>P''"'^2 

NORMAL  FORCE 

FP(3)  =  MASS^'^U-'-Q  -  MASS^V'^^P  -  MASS^'^XG^P^R  -  MASS*YG*Q''fR  + 
&  MASS''^ZG->P**2  +  MASS*ZG'^Q'^'>2  +  (RH0/2)*L'>M''f(  ZPP'"'P**2  + 
&  ZPR''^P^>R  +  ZRR''^R''"'^2)  +  (RH0/2)*L'V'V3^f(  ZQ*U'>Q  +  ZVP*V''^P  + 
&  ZVR'W^R)  +(RH0/2)*L''^'>2'nZW'^U'>W  +  ZVV'W''"'^2  +  U''"'^2'^(  ZDS* 
&  DS+ZDB''>-DB))-NORPIT+(WEIGHT-BOY)'^COS(THETA)''^COS(PHI)  + 
&  (RH0/2)--"^L^'-''-3'>ZQN-'^U''^Q''^EPS  +(RH0/2)''^L**2*(  ZWN*U-'^W  +ZDSN* 
&       U''""^2-^DS)'>EPS 

ROLL  FORCE 

FP(4)  =  -IZ'>Q'''R  +IY'>Q*R  -IXY''^P*R  +IYZ*Q''"'^2  -IYZ'VR''"'^2  +IXZ''^P'>Q  + 
&       MASS^'^YG-"-U'''Q  -MASS'^YG-"-V'^P  -MASS''^ZG'''W'^P+(RH0/2)*L^'">5-'KKPQ* 
6c       P'>Q  +  KQR'''Q*R)  +(RH0/2)''^L*'''4'HKP*U*P  +KR'>U'VR  +  KVQ'W-Q  + 
&       KWP'^W'^P  +  K\\'R''^W"R)  +(RH0/2)*L*-''O'HKV*U''-V  +  KVW'W''-W)  + 
&       (YG-'^WEIGHT  -  YB^'--BOY)''^COS(THETA)-'-COS(PHI)  -  (ZG^VEIGHT  - 
&       ZB-->BOY)-''--COS(THETA)'>SIN(PHI)  +  (RH0/2)'''L*''^4''^KPN''^U'>P'''EPS+ 
&       (  RH0/2)^'>-L''«"''O-'^U-'"'^2''-KPR0P  +MASS''^ZG^'^U-'>R 

PITCH  FORCE 

FP(5)  =  -IX'-P'-R  +IZ''^P'>R  +IXY''^Q''^R  -IYZ''^P'''Q  -IXZ''^P''"'^2  +IXZ''^R-'"''2  - 
&       MASS--^XG-'>U--^Q  +  MASS^'^XG-''-V''^P  +  MASS'-^ZG^'-V^'^R  -  MASS-'^ZG-^'W-Q  + 
&       (  RHO/  2 )  -VL-'"--5--'- (  MPP'">-p-'"'-2  +MPR''^P-''^R  +MRR--'-R--'-*2 )  +( RHO/  2  )-''L--'"'^4'V 
&       (MQ-'^U-'^Q  +  MVP''-V'>P  +  MVR*V"R)  +  (RH0/2)'"fL''-''3*(MW"U'>-W  + 
&       MVV---V— 2+U''""-2''^(MDS"DS+MDB''-DB))+  NORPIT  -(XG^'-'WEIGHT- 
&       XB--'-B0Y)'>C0S( THETA)-'^COS(  PHI  )+(RH0/2)''^L-'"'^4'VMQN''--U-"-Q--'-EPS  + 
&       (  RHO/ 2  ) '>L-->-'--3-'H  M\v'N''--U-"-W+MDSN-'-U-''^--'^2-'^DS )  --EPS - 
&       (ZG-'^WEIGHT-ZB-'^BOY)''^SIN(THETA) 

YAW  FORCE 

FP(6)  =  -IY''^P*Q  +IX'>P''^Q  +IXY'VP*'^2  -IXY''^Q*''^2  +IYZ''^P''^R  -IXZ''^Q'''R  - 
6c       MASS-'^XG-'^U^^R  +  MASS''^XG'>W'-P  -  MASS''-YG-'W--'-R  +  MASS^'-YG'^W-'Q  + 
6t       (RHO/2)''^L''"'^5*(NPQ-"-P-'-Q  +  NQR'^Q-"-R)  +(RH0/2)''^L-'>-''^4^>(NP''-U-''-P+ 
&       NR-"--U-''^R  +  NVQ'>V'''Q  +NWP'VW''fP  +  NWR-W'-^R)  +(RH0/2)''-L-'^''^3'HNV''f 
&       U-->V  +  NVlV-'-'V-'-W  +  NDR-->U----^>2-'^DR)  -  LATYAW  +  (XG-'-'WEIGHT  - 
&       XB-"-BOY)-''^COS(THETA)->SIN(PHI)  +  (YG-"-WEIGHT)'"^SIN(THETA) 
&       +(RH0/2)-'>L-"---'^3-"-U-'"'^2'>NPR0P-YB*B0Y-''^SIN(THETA) 


NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 

DO  600  J  =  1,6 

F(J)  =  0.  0 
DO  600  K  =  1,6 


F(J)  =  XMMINV(J,K)*FP(K)  +  F(J) 
600   CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

FIRST  SET  THE  DRIFT  CURRENT  VALUES 

UCO  =  0.  0 
VCO  =  0.  0 
WCO  =  0.0 

INERTIAL  POSITION  RATES  F(7-9) 

F(7)  =  UCO  +  U''^COS(PSI)'VCOS(THETA)  +  V''KCOS(PSI)*SIN(THETA)* 
&       SIN(PHI)  -  SIN(PSI)''^COS(PHI))  +  W'nCOS(PSI)'"^SIN(THETA)* 
&       COS(PHI)  +  SIN(PSI)*SIN(PHI)) 

F(8)  =  VCO  +  U-'^SIN(PSI)'VCOS(THETA)  +  V'-'K  SIN(  PSI)''^SIN(THETA)''^ 
6e       SIN(PHI)  +  COS(PSI)-'>COS(PHI))  +  W'--(SIN(PSI)'^SIN(THETA)* 
6c       COS(PHI)  -  COS(PSI)''>SIN(PHI)) 

F(9)  =  WCO  -  U*SIN(THETA)  +V''^COS(THETA)''^SIN(PHI)  +W''^COS(THETA)'V 
6c       COS(PHI) 

EULER  ANGLE  RATES  F( 10-12) 

F(10)  =  P  +  Q-'-SIN(PHI)''-TAN(THETA)  +  R-'>COS(  PHI)''^TAN(THETA) 

F(ll)  =  Q'VCOS(PHI)  -  R-->SIN(PHI) 

F(12)  =  Q*SIN(PHI)/COS(THETA)  +  R''--COS(PHI)/COS(THETA) 


UDOT  =  F(l 
VDOT  =  F(2 
WDOT  =  F(3 
PDOT  =  F(4 
QDOT  =  F(5 
RDOT  =  F(6 
XDOT  =  F(7 
YDOT  =  F(8 
ZDOT  =  F(9 
PHIDOT  =  F 
THETAD  =  F 
PSIDOT  =  F 


10) 

11) 

12) 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C       CREATE  OUTPUT  DATA  FILE 

c 

C  TESTER=MOD(FLOAT(I),100.  ) 

C  IF  (TESTER  .  EQ.  0.0)  THEN 

TIMER=FL0AT(I)/2. 

C  WRITE  (8,730)   DS ,DR,XPOS ,YPOS , ZPOS , ROLL, PITCH, YAW 

C  WRITE  (8,730)   FP(5) 

C  WRITE  (8,730)   U,V,W,P,Q,R 

C  WRITE  (8,730)   IX, IZ , IXY , lYZ , IXZ 

C  WRITE  (8,730)   MASS,XG,ZG 
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c 
c 
c 
c 
c 
c 

C20 
C 

C30 
710 


WRITE  (8,730)   RHO,L,MPP ,MPR,MRR 

WRITE  (8,730)   MQ , MVP , MVR , MW 

WRITE  (8,730)   MVV,MDS ,DS ,MDB ,DB ,NORPIT, WEIGHT 

WRITE  (8,730)   XB , BOY,THETA,PHI ,MQN,EPS 

WRITE  (8,730)   MWN,MDSN,ZB 

WRITE  (8,730)   TIME , DEPTH, DS ,THETA, S 

FORMAT  ( IX, 'TIME' ,3X.' COMMAND  DEPTH' , 3X, ' DIVE  P  ANG' , 
''^3X, 'PITCH  ANGLE' ,3X,*  SLIDING  SURFACE'/) 

FORMAT  (IX, F6.  2,2X,E11.  3  3X,E11.  3,4X,E10.  3,3X  Ell.  3) 

FORMAT  (IX,'    TIME',3X,'   U  ',3X,'   ETA',3X,'   XPROP' 
■>''3X,       'DS') 

WRITE  (8,730)  TIME ,U, ETA, XPROP, DS 
730   FORMAT  ( 1X,F6. 2, 2X,F6. 2 ,2X,F6. 3, 2X,F8.  4 , 2X,E11.  3) 
C     ENDIF 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 
C 
C 


FIRST  ORDER  INTEGRATION 
U  =  U  +  DELT^'fUDOT 
V  =  V  +  DELT'WDOT 
W  =  W  +  DELT^'^WDOT 
P  =  P  +  DELT''^PDOT 
Q  =  Q  +  DELT''^QDOT 
R  =  R  +  DELT--RDOT 
XPOS  =  XPOS  +  DELT-'^XDOT 
YPOS  =  YPOS  +  DELT---YDOT 
ZPOS  =  ZPOS  +  DELT--'-ZDOT 
PHI  =  PHI  +  DELT-'^PHIDOT 
THETA  =  THETA  +  DELT-'^THETAD 
PSI  =  PSI  +  DELT'^PSIDOT 


U  =  SURGE  RATE 

V  =  SWAY  RATE 
W  =  HEAVE  RATE 

P  =  ROLL  RATE 
Q  =  PITCH  RATE 
R  =  YAW  RATE 
X  =  SURGE 

Y  =  SWAY 
Z  =  HEAVE 
PHI  =  ROLL 
THETA  =  PITCH 
PSI  =  YAW 


,V^ryrVr,V.V^,V,Wr   OBSERVER  OF  SLIDING  CONTROLLER   Vr^rVrV.-Vf^yr^fyfVcVr 

CALL  OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , DELT , ZPOS , DS , UO ) 


V"VVr,vvr,v,vv.-,v,v,v  SLIDING  MODE  CONTROL  INPUT  *-v*;^^ryr,v,v,v,v--v,v 

S=QHAT+0.  75^-THAT-O.  0233'>(  ZHAT-COMZ  ) 

IF  (ABS(S)  .  LT.  BAR)  SAT=(  S/BAR)--'-ABS(  S) 

IF  (S  . LE.  -BAR)  SAT=-1 

IF  (S  .GE.  BAR)   SAT=1 

UHAT=1.  4286--'-QHAT+3.  1429*THAT 

UBAR=EITA-'-SAT 

DE=UHAT+UBAR 

IF  (DE  .GE.  0.4)  DS=0.  4 
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IF  (DE  .  LT.  -0.4)  DS=-0.4 

IF(  (DE  .LT.  0.4)  .AND.  (DE  .  GE. -0. 4)  )  DS=DE 

DB=-DS-'a.  0 
TIME=TIME+DELT 

PHIANG  =  PHI/0.0174532925 
THEANG  =  THETA/0.  0174532925 
PSIANG  =  PSI/0. 0174532925 

TRAC=-YPOS 
ROLL=PHIANG 
YAW=PSIANG 
DEPTH=-ZPOS 
DEPTH=ZPOS 
PITCH=THEANG 
BOWANG=(DB/. 01745) 
STNANG=(DS/. 01745) 


100   CONTINUE 
STOP 
END 

C      ,v,v,w>";r,v--v,v  OBSERVER  SUBROUTINE  Vr>v,v,v,v,v,v 

SUBROUTINE  OBSER(QHADOT,THADOT, ZHADOT,QHAT,THAT, ZHAT,DELT, 
''^ZPOS,DS,U) 
C 

QHADOT=-0.  7'VQHAT-O.  03''^THAT-0.  035''^DS-20.  9293'HZPOS-ZHAT) 

THADOT=QHAT-14.4092-->(ZPOS-ZHAT) 

ZHAD0T=-6->THAT+16.  45-"-(  ZPOS-ZHAT) 
C 

QHAT=QHAT+DELT-'>QHADOT 

THAT=THAT+DELT--^THADOT 

ZHAT=ZHAT+DELP'^ZHADOT 

RETURN 

END 
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